ICP代碼分析

ICP算法的介紹

ICP（Iterative Closest Point），即最近點迭代算法，是最爲經典的數據配准算法。其特徵在於，通過求取源點雲和目標點雲之間的對應點對，基於對應點對構造旋轉平移矩陣，並利用所求矩陣，將源點雲變換到目標點雲的座標系下，估計變換後源點雲與目標點雲的誤差函數，若誤差函數值大於閥值，則迭代進行上述運算直到滿足給定的誤差要求.

ICP算法採用最小二乘估計計算變換矩陣，原理簡單且具有較好的精度，但是由於採用了迭代計算，導致算法計算速度較慢，而且採用ICP進行配準計算時，其對待配準點雲的初始位置有一定要求，若所選初始位置不合理，則會導致算法陷入局部最優。。

PCL裏面的源碼分析

我接下來對pcl裏面的源碼瞭解了下，大體有些地方做了備註，但是未必萬全正確。

• 首先要介紹的是主體的ICP啓動程序：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the CloudIn data
  cloud_in->width    = 5;
  cloud_in->height   = 1;
  cloud_in->is_dense = false;
  cloud_in->points.resize (cloud_in->width * cloud_in->height);
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
  {
    cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"
      << std::endl;
  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " <<
      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
      cloud_in->points[i].z << std::endl;
  *cloud_out = *cloud_in;
  std::cout << "size:" << cloud_out->points.size() << std::endl;
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
  std::cout << "Transformed " << cloud_in->points.size () << " data points:"
      << std::endl;
  for (size_t i = 0; i < cloud_out->points.size (); ++i)
    std::cout << "    " << cloud_out->points[i].x << " " <<
      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(cloud_in);
  icp.setInputTarget(cloud_out);
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;

 return (0);
}

其中主要的功能就是在align（）這個函數中實現的。這個函數的大體位置是
registration/include/pcl/registraion/impl/registration.hpp這裏。代碼接下如下

• align（）函數

//函數裏調用這個真正的函數
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
{
  //分配input點雲的下標，函數在common/include/pcl/impl/pcl_base.hpp
  if (!initCompute ()) 
    return;

  // Resize the output dataset
  //如果output的下標數量和input不一樣，那就成爲一樣的
  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
  //這裏的output就是final，也就是最後由input轉化過來的點雲，不是匹配的目標點雲
  //因爲沒有被初試化的，所以直接拷貝點雲
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i] = input_->points[(*indices_)[i]];

  // 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
  //其實因爲座標是齊次座標，所以第四個元素是1，前面三個元素是x,y,z
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i].data[3] = 1.0;

  //實現的icp.hpp裏面，這個函數是重載函數，所以要找對
  //變種icp的更改基本都在這裏，改動h,hpp文件，以及改動主要的computeTransformation函數，前面的都是預備工作，關係不大
  computeTransformation (output, guess);

  //這個函數僅僅是返回一個布爾值true
  deinitCompute ();
}

在這個align裏面的最重要的函數就是computeTransformation (output, guess)。而這個函數就在registration/include/pcl/registraion/icp.hpp這裏。

• computeTransformation（）函數

#ifndef PCL_REGISTRATION_IMPL_ICP_HPP_
#define PCL_REGISTRATION_IMPL_ICP_HPP_

#include <pcl/registration/boost.h>
#include <pcl/correspondence.h>

///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud ( 
    const PointCloudSource &input, 
    PointCloudSource &output, 
    const Matrix4 &transform)
{
  //這裏的input和output在第一次的時候還是相同的值
  //但是在第二次迭代的時候纔是正常的步驟
  Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
  Eigen::Matrix4f tr = transform.template cast<float> ();

  // XYZ is ALWAYS present due to the templatization, so we only have to check for normals
  if (source_has_normals_)
  {
    Eigen::Vector3f nt, nt_t;
    Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);

    for (size_t i = 0; i < input.size (); ++i)
    {
      將input的數據填充到pt裏
      const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
      uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
      memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
      memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
      memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));

      if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) 
        continue;

      //這裏就是轉換的公式，是齊次的轉換
      pt_t = tr * pt;
      
      //把pt_t的值給data_out
      memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
      memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
      memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));

      memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float));
      memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float));
      memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float));

      if (!pcl_isfinite (nt[0]) || !pcl_isfinite (nt[1]) || !pcl_isfinite (nt[2])) 
        continue;
      //這裏是非齊次的轉換
      nt_t = rot * nt;

      //把轉換後的nt_t給data_out
      memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float));
      memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float));
      memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float));
    }
  }
  else
  {
    for (size_t i = 0; i < input.size (); ++i)
    {
      const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
      uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
      memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
      memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
      memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));

      if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) 
        continue;

      //這裏是齊次的轉換
      pt_t = tr * pt;

      memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
      memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
      memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
    }
  }
  
}

///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
    PointCloudSource &output, const Matrix4 &guess)
{
  // Point cloud containing the correspondences of each point in <input, indices>
  //input_transformed是input被轉換一次之後的點雲
  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
     //在icp.hpp裏48行
    transformCloud (*input_, *input_transformed, guess);
  }
  else
    //否則就是直接複製，其實這裏還沒有開始轉換，因爲input_transformed還是原來的input
    *input_transformed = *input_;
 
  transformation_ = Matrix4::Identity ();

  // Make blobs if necessary
  //我也不知道這個步驟的含義，要製造異常點嗎？
  determineRequiredBlobData ();
  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_);
  if (correspondence_estimation_->requiresTargetNormals ())
    correspondence_estimation_->setTargetNormals (target_blob);
  // Correspondence Rejectors need a binary blob
  for (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);
  }

  //MSE是均方誤差，這裏是設置迭代的相關參數
  convergence_criteria_->setMaximumIterations (max_iterations_);
  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
  if (transformation_rotation_epsilon_ > 0)
    convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
  else
    convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);

  // Repeat until convergence
  //該方法的主體是一個do-while循環，查找最近點，剔除錯誤的對應點，收斂原則都在這裏
  //correspondence_estimation_ 、correspondence_rejectors_ 和 convergence_criteria_
  //這三個變量的作用分別代表查找最近點，剔除錯誤的對應點，收斂準則
  //因爲是do-while循環，因此收斂準則的作用是跳出循環
  //transformation_estimation_是求解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
    //尋找迭代點雲的對應點
    //use_reciprocal_correspondence_是相反的對應關係
    if (use_reciprocal_correspondence_)
      //determineReciprocalCorrespondences（）在correspondence_estimation.hpp文件裏，corr_dist_threshold_是最大距離
      correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
    else
      correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);

    //if (correspondence_rejectors_.empty ())
    //把已經有對應關係的correspondences_初始化temp_correspondences，當然這是一個動態的暫時內存
    CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
    for (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_;
    }

    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;
    }
    //在前面的尋找一致性估計後（尋找對應點後），接下來的步驟又是主要的函數步驟，transformation_estimation_是求解ICP的具體算法
    // Estimate the transform
    //查看transformation_estimation_svd.hpp中的TransformationEstimationSVD類的estimateRigidTransformation 方法
    //這裏就是target_是最終的目標點雲，在迭代過程中不變，但是input_transformed總是會不停的更新，直到和目標重合
    transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);

    // Tranform 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_));
  }
  while (!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
  //先把input_複製過去，然後在將轉換後的點雲疊加上去，至此，算法完成
  transformCloud (*input_, output, final_transformation_);
}

template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
{
  need_source_blob_ = false;
  need_target_blob_ = false;
  // Check estimator
  //僅僅是檢查而已，requiresSourceNormals函數返回true
  // |=是複合的邏輯或
  need_source_blob_  |= correspondence_estimation_->requiresSourceNormals ();
  need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
  // Add warnings if necessary
  if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
  {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
  }
  if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
  {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
  }
  // Check rejectors
  for (size_t i = 0; i < correspondence_rejectors_.size (); i++)
  {
    registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
    need_source_blob_ |= rej->requiresSourcePoints ();
    need_source_blob_ |= rej->requiresSourceNormals ();
    need_target_blob_ |= rej->requiresTargetPoints ();
    need_target_blob_ |= rej->requiresTargetNormals ();
    if (rej->requiresSourceNormals () && !source_has_normals_)
    {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
    }
    if (rej->requiresTargetNormals () && !target_has_normals_)
    {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
    }
  }
}

///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud (
    const PointCloudSource &input, 
    PointCloudSource &output, 
    const Matrix4 &transform)
{
  pcl::transformPointCloudWithNormals (input, output, transform);
}
      

#endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */

當初的代碼註釋，但是也是依照個人理解初次寫，如果有問題，還請見諒！