Skip to content

Alignment

Principle

  • In the process of aligning multiple clouds, it is necessary to first align them roughly before proceeding to a fine registration.
  • The rough registration is based on features identification, that find points mathcing in both clouds.

Rough alignment

Estimating normals

The first step is to estimate normals of a point :

  1. For each point, neighbors are detected
  2. Knowing their positions, a "surface" can be estimated

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

  4. With solely those data, it is not possible to know which orientation the normal has to go.

  5. It is necessary to know the viewpoint (from which data has been acquired). Normals estimations

The corresponding method is the following :

/**
* @brief Estimate normals of given point cloud under given search radius
* @tparam PointT Should be a pcl::PointXYZxxx
* @param incloud Cloud to compute normals from
* @param normals Output normals
* @param radius Search sphere radius for normals estimation
* @param verbose If has to output debug on console
*/
template <typename PointT>
void estimateNormals(typename pcl::PointCloud<PointT>::Ptr& incloud, pcl::PointCloud<pcl::Normal>::Ptr& normals, float radius, bool verbose = 0);
* The sphere 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 :

pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
estimateNormals<UsedPoint>(mycloud, normals, 1.2);
Normals view

Fast Point Feature Histograms

  • Once normals are acquired, it is possible to find features on the parts that will be used to match roughly multiple parts. The PFH goal is to detect mean curvature around a point, providing a stronger signature than a normal can be.
  • The FPFH is a quicker (but less accurate) PFH algorithm.

The corresponding method is the following :

/**
* @brief Fast Point Feature Histograms
* @tparam PointT Should be a pcl::PointXYZxxx
* @param incloud Input cloud
* @param normals Input normals
* @param outsignature Signature output
* @param radius Search sphere radius (must be larger thant the one used for normal estimation)
* @param verbose If has to output debug on console
*/
template <typename PointT>
void 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 :

pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_signature(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh<pcl::PointXYZ>(mycloudb, normals, cloud_signature, 3);
When plotted, we have an histogram reprensenting signature for each point. FPFH plot

Correspondences estimation

  • Once features have been computed, they can be compared to find match between both clouds.
  • This method may take some time, as all features are compared to find all match. It may lead to bad correspondences. Correspondences estimation

The corresponding method is the following :

/**
* @brief Estimate correspondance between two sets of features
* @tparam CorrespT Signature type
* @param sourcecloud Source to align with target
* @param targetcloud Target to align source on
* @param corresp Corresponding points
* @param verbose If has to output debug on console
*/
template <typename CorrespT>
void 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 : 
pcl::Correspondences correspondences;
fpfhCorrespondenceEstimation<pcl::FPFHSignature33>(compCloudFPFH, baseCloudFPFH, correspondences);

Rejecting bad correspondences

  • As seen above, we find many bad correspondences that will lead to a bad rough alignment.
  • We can reject bad ones thanks to the RANSAC method, while getting the T/R matrix that would allow to align them. Bad correspondences rejected

The corresponding method is the following :

/**
* @brief Given set of correspondences, reject bad ones
* @tparam PointT Should be a pcl::PointXYZxxx
* @param sourcecloud Cloud to align with target
* @param targetcloud Target to align source on
* @param all_corresp Previsouly calculated correspondences
* @param good_corresp Output of good correspondences
* @param inlier_threshold Maximum distance between corresponding points
* @param iterations Number of RANSAC iterations
* @param verbose If has to output debug on console
* @return Rough transformation matrix
*/
template <typename PointT>
Eigen::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);
  • The 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 :

pcl::Correspondences goodCorresp;
Eigen::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 :

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

Rougly aligned clouds
Figure 1: Rougly aligned clouds

Display matrix

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

/**
* @brief Display Eigen Matrix with rotation and translation infos
* @tparam MatrixT Matrix type
* @param matrix Matrix to display
*/
template <typename MatrixT>
void print4x4Matrix(const typename MatrixT& matrix);
That can be called like : 
print4x4Matrix<Eigen::Matrix4f>(rough_transf_matrix);

Fine alignment

  • Once clouds are roughly aligned, it is possible to use the Iterative Closest Point algorithm to align clouds very closely one to another.
  • This algorithm requires both clouds to be aligned, but also that the proportions are the same and no cloud is distorted.

  • Its principle is :

  • 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.

/**
* @brief Search for a transformation matrix given two clouds thanks to ICP
* @tparam PointT Should be a pcl::PointXYZxxx
* @param target_cloud The cloud that we want to align to the target
* @param toalign_cloud The cloud that we want to be aligned on
* @param maxIterations Maximum ICP iterations (10 - 100 normally)
* @param scanRes Distance between points (f.ex. if downsampled with voxel, leaf size)
* @param fitnessEpsilon Fitness score we target
* @param minEpsilon Minimal difference we expect between two consecutive alignments
* @param verbose If has to output debug on console
* @return fine transformation matrix
*/
template<typename PointT>
Eigen::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 :

  1. maxIterations criterion tells how many iterations, at maximum, can be done
  2. fitnessEpsilon is the fitness score under which the alignment is sufficent (1e-8 is a good score)
  3. 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 :

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