# Alignment#

## Principle#

## Rough alignment#

### Estimating normals#

The first step is to estimate normals of a point :

For each point, neighbors are detected

Knowing their positions, a “surface” can be estimated

A normal to the surface passing through the point can be estimated

The corresponding method is the following :

```
1/**
2* @brief Estimate normals of given point cloud under given search radius
3* @tparam PointT Should be a pcl::PointXYZxxx
4* @param incloud Cloud to compute normals from
5* @param normals Output normals
6* @param radius Search sphere radius for normals estimation
7* @param verbose If has to output debug on console
8*/
9template <typename PointT>
10void estimateNormals(typename pcl::PointCloud<PointT>::Ptr& incloud, pcl::PointCloud<pcl::Normal>::Ptr& normals, float radius, bool verbose = 0);
```

**radius**correspond to the distance to look neighbors for.

*This method does not handle the viewpoint, that is set to (0,0,0) by default*.

To use it, do as follow :

```
1pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
2estimateNormals<UsedPoint>(mycloud, normals, 1.2);
```

### Fast Point Feature Histograms#

The corresponding method is the following :

```
1/**
2* @brief Fast Point Feature Histograms
3* @tparam PointT Should be a pcl::PointXYZxxx
4* @param incloud Input cloud
5* @param normals Input normals
6* @param outsignature Signature output
7* @param radius Search sphere radius (must be larger thant the one used for normal estimation)
8* @param verbose If has to output debug on console
9*/
10template <typename PointT>
11void fpfh(typename pcl::PointCloud<PointT>::Ptr& incloud, pcl::PointCloud<pcl::Normal>::Ptr& normals, pcl::PointCloud<pcl::FPFHSignature33>::Ptr& outsignature, float radius, bool verbose = 0);
```

The sphere **radius** correspond to the distance to look neighbors for curvature estimation. It must be larger than the one used for normals estimation, else it would not be accurate at all.

To use it, do as follow :

```
1pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_signature(new pcl::PointCloud<pcl::FPFHSignature33>());
2fpfh<pcl::PointXYZ>(mycloudb, normals, cloud_signature, 3);
```

When plotted, we have an histogram reprensenting signature for each point.

### Correspondences estimation#

The corresponding method is the following :

```
1/**
2* @brief Estimate correspondance between two sets of features
3* @tparam CorrespT Signature type
4* @param sourcecloud Source to align with target
5* @param targetcloud Target to align source on
6* @param corresp Corresponding points
7* @param verbose If has to output debug on console
8*/
9template <typename CorrespT>
10void fpfhCorrespondenceEstimation(typename pcl::PointCloud<CorrespT>::Ptr& sourcecloud, typename pcl::PointCloud<CorrespT>::Ptr& targetcloud, pcl::Correspondences& corresp, bool verbose = 0);
```

To use it, do as follow :

```
1pcl::Correspondences correspondences;
2fpfhCorrespondenceEstimation<pcl::FPFHSignature33>(compCloudFPFH, baseCloudFPFH, correspondences);
```

### Rejecting bad correspondences#

The corresponding method is the following :

```
1/**
2* @brief Given set of correspondences, reject bad ones
3* @tparam PointT Should be a pcl::PointXYZxxx
4* @param sourcecloud Cloud to align with target
5* @param targetcloud Target to align source on
6* @param all_corresp Previsouly calculated correspondences
7* @param good_corresp Output of good correspondences
8* @param inlier_threshold Maximum distance between corresponding points
9* @param iterations Number of RANSAC iterations
10* @param verbose If has to output debug on console
11* @return Rough transformation matrix
12*/
13template <typename PointT>
14Eigen::Matrix4f rejectBadCorrespondences(typename pcl::PointCloud<PointT>::Ptr& sourcecloud, typename pcl::PointCloud<PointT>::Ptr& targetcloud, pcl::Correspondences& all_corresp, pcl::Correspondences& good_corresp, float inlier_threshold, unsigned int iterations, bool verbose = 0);
```

**inlier_threshold**set the distance RANSAC will look for neighbors correspondences.

**iterations**tell RANSAC how many iterations should be done.

To use it, do as follow :

```
1pcl::Correspondences goodCorresp;
2Eigen::Matrix4f rough_transf_matrix = rejectBadCorrespondences<PointXYZ>(compCloud, baseCloud , correspondences, goodCorresp, 3, 5000);
```

#### Align with matrix#

Given matrix can be used to align both cloud, thanks to :

```
1pcl::transformPointCloud(*compCloud, *roughAlignedCloudOutput, rough_transf_matrix);
```

#### Display matrix#

If you want to check matrix values, following method is implemented :

```
1/**
2* @brief Display Eigen Matrix with rotation and translation infos
3* @tparam MatrixT Matrix type
4* @param matrix Matrix to display
5*/
6template <typename MatrixT>
7void print4x4Matrix(const typename MatrixT& matrix);
```

That can be called like :

```
1print4x4Matrix<Eigen::Matrix4f>(rough_transf_matrix);
```

## Fine alignment#

For each point in the source cloud, match the closest one to the target cloud

Estimate transformation matrix

Transform the source

Reiterate

Multiple variants exist, most suitable in a case or another. Default PCL ICP algorithm is used here.

```
1/**
2* @brief Search for a transformation matrix given two clouds thanks to ICP
3* @tparam PointT Should be a pcl::PointXYZxxx
4* @param target_cloud The cloud that we want to align to the target
5* @param toalign_cloud The cloud that we want to be aligned on
6* @param maxIterations Maximum ICP iterations (10 - 100 normally)
7* @param scanRes Distance between points (f.ex. if downsampled with voxel, leaf size)
8* @param fitnessEpsilon Fitness score we target
9* @param minEpsilon Minimal difference we expect between two consecutive alignments
10* @param verbose If has to output debug on console
11* @return fine transformation matrix
12*/
13template<typename PointT>
14Eigen::Matrix4f icpAlignment(typename pcl::PointCloud<PointT>::Ptr& target_cloud, typename pcl::PointCloud<PointT>::Ptr& toalign_cloud, unsigned int maxIterations = 10, float scanRes = 0, float fitnessEpsilon = 0, float minEpsilon = 0, bool verbose = false);
```

ICP may give very good results, or none. Three criteria allow to finish the registration :

*maxIterations*criterion tells how many iterations, at maximum, can be done*fitnessEpsilon*is the fitness score under which the alignment is sufficent (1e-8 is a good score)*minEpsilon*is the minimal difference we expect between two consecutive alignments, to prevent divergence

*scanRes* allow us to tell the approximative distance between points, following the scanner resolution or the downsampling size. A too low value will not allow registration, while a too big may not give results.

It can be called like :

```
1Eigen::Matrix4f icp_matrix = icpAlignment<UsedPoint>(transformedCompCloud, baseCloudDS, 50, 0.03, 1e-8, 1e-6);
```