PCL-ICP(IterativeClosestPoint)源码解析

1.PCL-ICP代码框架

话不多说，直接上代码

    #include <pcl/registration/icp.h>
    #include <pcl/point_types.h>
    pc_xyz::Ptr result = boost::make_shared<pc_xyz>();
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setMaxCorrespondenceDistance(max_correspondence_dist_);// 设置corr_dist_threshold_
    icp.setMaximumIterations(max_iterations_); //设置max_iterations_
    icp.setTransformationEpsilon(1e-6); //设置transformation_epsilon_
    icp.setEuclideanFitnessEpsilon(1e-2); // 设置euclidean_fitness_epsilon_
    icp.setInputSource(local_map_xyz);//设置input_ ，source_cloud_updated_ = true;
    icp.setInputTarget(global_map_xyz);//设置target_， target_cloud_updated_ = true;
    if (do_ransac_outlier_rejection_) {
   
   
        icp.setRANSACOutlierRejectionThreshold(ransac_outlier_rejection_threshold_);
        icp.setRANSACIterations(ransac_iterations_);
    }
    icp.align(*result);//主要匹配函数
    is_converged = icp.hasConverged();
    fitness = icp.getFitnessScore();
    correction = icp.getFinalTransformation();

2. pcl::IterativeClosestPoint类格

3. pcl::Registration::align()详解

第1小节中的

  icp.align(*result);//主要匹配函数

对应的为pcl/registtation/impl/registation.hpp中的

template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
{
   
   
align (output, Matrix4::Identity ());
}

其调用pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess) 函数。
ICP的核心内容也就是该函数，接下来我们对其进行详细解释：

template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (Point