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 (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
}

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_
}

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;
        };

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
}

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);
}