PCL-ICP(IterativeClosestPoint)源码解析_27804-iterative-closest-point

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();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

2. pcl::IterativeClosestPoint类格

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

第1小节中的

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

对应的为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 ());
}
1
2
3
4
5

其调用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 (PointCloudSource &output, const Matrix4& guess)
{
  if (!initCompute ()) //初始化kd树，及点云索引
    return;

  // Resize the output dataset
  if (output.points.size () != indices_->size ())
    output.points.resize (indices_->size ());
  // Copy the header
  output.header   = input_->header;
  // Check if the output will be computed for all points or only a subset
  if (indices_->size () != input_->points.size ())
  {
    output.width    = static_cast<uint32_t> (indices_->size ());
    output.height   = 1;
  }
  else
  {
    output.width    = static_cast<uint32_t> (input_->width);
    output.height   = input_->height;
  }
  output.is_dense = input_->is_dense;

  // Copy the point data to output
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i] = input_->points[(*indices_)[i]];//将原始点云复制到output中

  // Set the internal point representation of choice unless otherwise noted
  if (point_representation_ && !force_no_recompute_) 
    tree_->setPointRepresentation (point_representation_);

  // Perform the actual transformation computation
  converged_ = false;
  final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();

  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 
  // transformation
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i].data[3] = 1.0;

  computeTransformation (output, guess);//求解旋转矩阵，动态绑定到pcl::IterativeClosestPoint::computeTransformation()

  deinitCompute ();// return (true); 直接返回true
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

3.1 pcl::Registration::initCompute()详解

initCompute用于初始化kd树，及点云索引

template <typename PointSource, typename PointTarget, typename Scalar>
bool
Registration<PointSource, PointTarget, Scalar>::initCompute()
{
  if (!target_) {
    PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
              getClassName().c_str());
    return (false);
  }

  // Only update target kd-tree if a new target cloud was set
  if (target_cloud_updated_ && !force_no_recompute_) {//force_no_recompute_默认初始化为false，因此第一次运行时，进入该if判断
    tree_->setInputCloud(target_);//用目标点云初始化kd树
    target_cloud_updated_ = false;
  }

  // Update the correspondence estimation
  if (correspondence_estimation_) {
    correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_);//初始化correspondence_estimation_，包括    tree_ = tree;    force_no_recompute_ = force_no_recompute;    target_cloud_updated_ = true;
        correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,
                                                      force_no_recompute_reciprocal_);
  }

  // Note: we /cannot/ update the search method on all correspondence rejectors, because
  // we know nothing about them. If they should be cached, they must be cached
  // individually.

  return (PCLBase<PointSource>::initCompute());//初始化与input_的size对应的indices_
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

3.2 pcl::IterativeClosestPoint::computeTransformation()详解

3.2.1 收敛状态简介

        enum ConvergenceState
        {
          CONVERGENCE_CRITERIA_NOT_CONVERGED,//默认值，converged_ = false;
          CONVERGENCE_CRITERIA_ITERATIONS,//迭代结果为达到迭代次数，此时若设置failure_after_max_iter_ = true，就会导致converged_ = false;默认状态是failure_after_max_iter_ = false，因此该情况下超过迭代次数会判断converged_ = true
          CONVERGENCE_CRITERIA_TRANSFORM,//平移距离的平方和满足transformation_epsilon_要求，则判断CONVERGENCE_CRITERIA_TRANSFORM，  converged_ = true
          CONVERGENCE_CRITERIA_ABS_MSE,//两次迭代的绝对距离差满足收敛要求，converged_ = true
          CONVERGENCE_CRITERIA_REL_MSE,//两次迭代的相对距离差满足收敛要求，converged_ = true
          CONVERGENCE_CRITERIA_NO_CORRESPONDENCES//迭代结果为对应点对太少，converged_ = false;
        };

1
2
3
4
5
6
7
8
9
10

3.2.2 computeTransformation代码解析

pcl::IterativeClosestPoint::computeTransformation() 位于pcl/registration/impl/icp.hpp文件中：

template <typename PointSource, typename PointTarget, typename Scalar>
void
IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
    PointCloudSource& output, const Matrix4& guess)
{
  // Point cloud containing the correspondences of each point in <input, indices>
  PointCloudSourcePtr input_transformed(new PointCloudSource);

  nr_iterations_ = 0;
  converged_ = false;

  // Initialise final transformation to the guessed one
  final_transformation_ = guess;

  // If the guessed transformation is non identity
  if (guess != Matrix4::Identity()) {
    input_transformed->resize(input_->size());
    // Apply guessed transformation prior to search for neighbours
    transformCloud(*input_, *input_transformed, guess);
  }
  else
    *input_transformed = *input_;

  transformation_ = Matrix4::Identity();

  // Make blobs if necessary
  determineRequiredBlobData();// 未指定correspondence_rejectors_ 以及点云都没有法向量的话，need_source_blob_以及need_target_blob_ 都是false，即不要求处理“二进制长对象（blob）”
  PCLPointCloud2::Ptr target_blob(new PCLPointCloud2);
  if (need_target_blob_)
    pcl::toPCLPointCloud2(*target_, *target_blob);

  // Pass in the default target for the Correspondence Estimation/Rejection code
  correspondence_estimation_->setInputTarget(target_); //设置CorrespondenceEstimationBase中的target_ ，target_cloud_updated_ = true；
  if (correspondence_estimation_->requiresTargetNormals())
    correspondence_estimation_->setTargetNormals(target_blob);
  // Correspondence Rejectors need a binary blob
  for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
    registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
    if (rej->requiresTargetPoints())
      rej->setTargetPoints(target_blob);
    if (rej->requiresTargetNormals() && target_has_normals_)
      rej->setTargetNormals(target_blob);
  }

  convergence_criteria_->setMaximumIterations(max_iterations_);//设置DefaultConvergenceCriteria 中的max_iterations_
  convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);//设置DefaultConvergenceCriteria 中的mse_threshold_relative_
  convergence_criteria_->setTranslationThreshold(transformation_epsilon_);//设置DefaultConvergenceCriteria 中的translation_threshold_
  if (transformation_rotation_epsilon_ > 0)
    convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
  else
    convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);//设置DefaultConvergenceCriteria 中的rotation_threshold_

  // Repeat until convergence
  //以下进入icp迭代解算
  do {
    // Get blob data if needed
    PCLPointCloud2::Ptr input_transformed_blob;
    if (need_source_blob_) {
      input_transformed_blob.reset(new PCLPointCloud2);
      toPCLPointCloud2(*input_transformed, *input_transformed_blob);
    }
    // Save the previously estimated transformation
    previous_transformation_ = transformation_;

    // Set the source each iteration, to ensure the dirty flag is updated
    correspondence_estimation_->setInputSource(input_transformed);
    if (correspondence_estimation_->requiresSourceNormals())
      correspondence_estimation_->setSourceNormals(input_transformed_blob);
    // Estimate correspondences
    if (use_reciprocal_correspondence_)
      correspondence_estimation_->determineReciprocalCorrespondences(
          *correspondences_, corr_dist_threshold_);
    else
      correspondence_estimation_->determineCorrespondences(*correspondences_,
                                                           corr_dist_threshold_);//基于kd树最近邻查找最近点。

    // if (correspondence_rejectors_.empty ())
    CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
    for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
      registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
      PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
                rej->getClassName().c_str());
      if (rej->requiresSourcePoints())
        rej->setSourcePoints(input_transformed_blob);
      if (rej->requiresSourceNormals() && source_has_normals_)
        rej->setSourceNormals(input_transformed_blob);
      rej->setInputCorrespondences(temp_correspondences);
      rej->getCorrespondences(*correspondences_);
      // Modify input for the next iteration
      if (i < correspondence_rejectors_.size() - 1)
        *temp_correspondences = *correspondences_;
    }

    std::size_t cnt = correspondences_->size();
    // Check whether we have enough correspondences
    if (static_cast<int>(cnt) < min_number_correspondences_) {
      PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
                "Relax your threshold parameters.\n",
                getClassName().c_str());
      convergence_criteria_->setConvergenceState(
          pcl::registration::DefaultConvergenceCriteria<
              Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
      converged_ = false;
      break;//若对应点对数太少，则直接终止icp，收敛状态为false
    }

    // Estimate the transform
    transformation_estimation_->estimateRigidTransformation(
        *input_transformed, *target_, *correspondences_, transformation_);//进行svd位姿解算

    // Transform the data
    transformCloud(*input_transformed, *input_transformed, transformation_);

    // Obtain the final transformation
    final_transformation_ = transformation_ * final_transformation_;//叠加位姿估算结果

    ++nr_iterations_;

    // Update the vizualization of icp convergence
    // if (update_visualizer_ != 0)
    //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );

    converged_ = static_cast<bool>((*convergence_criteria_));//这里显式类型转换调用了pcl::registration::ConvergenceCriteria中对bool的重载，该重载中调用了pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged（）函数
  } while (convergence_criteria_->getConvergenceState() ==
           pcl::registration::DefaultConvergenceCriteria<
               Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);

  // Transform the input cloud using the final transformation
  PCL_DEBUG("Transformation "
            "is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%"
            "5f\t%5f\t%5f\t%5f\n",
            final_transformation_(0, 0),
            final_transformation_(0, 1),
            final_transformation_(0, 2),
            final_transformation_(0, 3),
            final_transformation_(1, 0),
            final_transformation_(1, 1),
            final_transformation_(1, 2),
            final_transformation_(1, 3),
            final_transformation_(2, 0),
            final_transformation_(2, 1),
            final_transformation_(2, 2),
            final_transformation_(2, 3),
            final_transformation_(3, 0),
            final_transformation_(3, 1),
            final_transformation_(3, 2),
            final_transformation_(3, 3));//打印最终的变换矩阵

  // Copy all the values
  output = *input_;
  // Transform the XYZ + normals
  transformCloud(*input_, output, final_transformation_);//进行坐标转换获取最终结果output
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153

3.2.3 pcl::registration::DefaultConvergenceCriteria::hasConverged ()代码解析

template <typename Scalar> bool
pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()
{
  convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;

  PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_);
  // 1. Number of iterations has reached the maximum user imposed number of iterations
  if (iterations_ >= max_iterations_)
  {
    if (failure_after_max_iter_)
      return (false);
    else
    {
      convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;
      return (true);
    }
    return (failure_after_max_iter_ ? false : true);
  }

  // 2. The epsilon (difference) between the previous transformation and the current estimated transformation
  double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);//罗德里格斯公式
  double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) +
                           transformation_.coeff (1, 3) * transformation_.coeff (1, 3) +
                           transformation_.coeff (2, 3) * transformation_.coeff (2, 3);//平移二范数
  PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave %f rotation (cosine) and %f translation.\n", cos_angle, translation_sqr);

  if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_)//平移距离的二范数满足要求则判断CONVERGENCE_CRITERIA_TRANSFORM
  {
    if (iterations_similar_transforms_ < max_iterations_similar_transforms_)
    {
      // Increment the number of transforms that the thresholds are allowed to be similar
      ++iterations_similar_transforms_;
      return (false);
    }
    else
    {
      iterations_similar_transforms_ = 0;
      convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM;
      return (true);
    }
  }

  correspondences_cur_mse_ = calculateMSE (correspondences_);//计算平均距离，单位为m
  PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: %f / %f.\n", correspondences_prev_mse_, correspondences_cur_mse_);

  // 3. The relative sum of Euclidean squared errors is smaller than a user defined threshold
  // Absolute
  if (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_)//两次迭代的绝对距离差满足收敛要求
  {
    if (iterations_similar_transforms_ < max_iterations_similar_transforms_)
    {
      // Increment the number of transforms that the thresholds are allowed to be similar
      ++iterations_similar_transforms_;
      return (false);
    }
    else
    {
      iterations_similar_transforms_ = 0;
      convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE;
      return (true);
    }
  }
  
  // Relative
  if (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_)//两次迭代的相对距离差满足收敛要求
  {
    if (iterations_similar_transforms_ < max_iterations_similar_transforms_)
    {
      // Increment the number of transforms that the thresholds are allowed to be similar
      ++iterations_similar_transforms_;
      return (false);
    }
    else
    {
      iterations_similar_transforms_ = 0;
      convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE;
      return (true);
    }
  }

  correspondences_prev_mse_ = correspondences_cur_mse_;

  return (false);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84