PCL_icp源码笔记

1. icp的使用用例

   pcl::IterativeClosestPoint<PointT, PointT> icp;
   	icp.setInputSource(cloud_source);
       icp.setInputTarget(cloud_target);
       icp.setTransformationEpsilon(1e-10);    // 为终止条件设置最小转换差异
       icp.setMaxCorrespondenceDistance(0.2);//设置对应点对之间的最大距离（大于该距离的点不考虑 m。
       icp.setEuclideanFitnessEpsilon(0.001);  // 设置收敛条件是均方误差和小于阈值， 停止迭代；
       icp.setMaximumIterations(35);           // 最大迭代次数
       icp.align(*cloud_icp,T);

2.  pcl源码

• 类的继承关系

 class Registration : public PCLBase<PointSource>
 class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>

•  处理过程

 基础函数 赋值indices_

1. registration.h。 Registration类构造函数初始化

2. icp.h 。 IterativeClosestPoint类构造函数初始化 ，指针对象transformation_estimation_，correspondence_estimation_，convergence_criteria_，初始化。

3. 调用ich.h中 setInputSource (const PointCloudSourceConstPtr &cloud) 函数 .
 override 可以用来修饰派生类中覆盖基类虚函数的成员函数，表示该成员函数必须是虚函数，且必须和基类函数具有相同的函数签名。这里覆盖Registraion类中的setInputSource函数。
 等赋值 。

source_cloud_updated_ = true

x_idx_offset_= 0

y_idx_offset_= 4

z_idx_offset_= 8

registration.h

PCLBase<PointSource>::setInputCloud (cloud); 将输入点云赋值input_

 4.调用icp.h中setInputTarget（）

       registration.hpp中setInputTarget (const PointCloudTargetConstPtr &cloud)。完成target点云传入

target_ = cloud;

target_cloud_updated_ = true

5.通过 registration.h 中以下函数实现参数读取

setTransformationEpsilon( )

setMaxCorrespondenceDistance( )

setEuclideanFitnessEpsilo( )

setMaximumIterations（ ）

6.registration.hpp中align函数实现。

    6.1 有初始变换矩阵传入赋值guess，无初始变换矩阵传入时，赋值初始变换为单位矩阵。

 6.2 调用initCompute()更新target kd_tree

target_cloud_updated_ = false;

correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_);

correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_);

传递kdtree指针到CorrespondenceEstimationBase类中tree_

6.4 初始化传入output 开始computeTransformation (output, guess);

output点云初始化为input_

初始化各变换矩阵

final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();

7.icp.hpp中computeTransformation（）

输入点云初始变换，同上guess 若guess矩阵为单位矩阵*input_transformed = *input_

反之根据变换矩阵得到input_transformed

  7.1determineRequiredBlobData ();

判断是否加入法向量？最终结果

need_target_blob_=0

need_source_blob_=0

correspondence_rejectors_.size ()=0

7.2 CorrespondenceEstimationBase类中tree_ 输入目标点云

correspondence_estimation_->setInputTarget (target_);

更新target_ = target_;

7.3 迭代收敛参数设置

convergence_criteria_->setMaximumIterations (max_iterations_);

convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);

convergence_criteria_->setTranslationThreshold (transformation_epsilon_);

7.4 循环迭代 直至收敛

correspondence_estimation_->setInputSource (input_transformed);

将input_transformed点云由setInputSource函数输入， input_=input_transformed

寻找对应点关系correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);

7.5 寻找input_ target_点云之间点对关系

1.initCompute() tree_输入点云

tree_->setInputCloud (target_);

2.tree_->nearestKSearch遍历input_寻找一个最近邻，将每个点对存入自定义pcl::Correspondences结构correspondences中

 7.6 由点对关系估计变换

// Estimate the transform

transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);

1.source_it target_it迭代器初始化

2.estimateRigidTransformation (source_it, target_it, transformation_matrix);重载函数调用

此处通过将点对关系的点放入动态矩阵中，调用pcl::umeyama接口直接求出变换关系transformation_matrix

7.7判断是否收敛

convergence_criteria_->getConvergenceState()