PCL之ICP演算法

朽木白露發表於2020-10-22

轉自：https://blog.csdn.net/u010696366/article/details/8941938?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param

先上例子, 下面是用PCL中的icp配準兩個點雲：

void PairwiseICP(const pcl::PointCloud<PointT>::Ptr &cloud_target, const pcl::PointCloud<PointT>::Ptr &cloud_source, pcl::PointCloud<PointT>::Ptr &output )
{
	PointCloud<PointT>::Ptr src(new PointCloud<PointT>);
	PointCloud<PointT>::Ptr tgt(new PointCloud<PointT>);

	tgt = cloud_target;
	src = cloud_source;

	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaxCorrespondenceDistance(0.1);
	icp.setTransformationEpsilon(1e-10);
	icp.setEuclideanFitnessEpsilon(0.01);
	icp.setMaximumIterations (100);

	icp.setInputSource (src);
	icp.setInputTarget (tgt);
	icp.align (*output);
//	std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;
		
	output->resize(tgt->size()+output->size());
	for (int i=0;i<tgt->size();i++)
	{
		output->push_back(tgt->points[i]);
	}
	cout<<"After registration using ICP:"<<output->size()<<endl;
}

原始碼pcl.h裡給出了Usage example，原始碼從github上下載之後可以在這個目錄找到
.\pcl-master\registration\include\pcl\registration。

這裡可以看到兩個重要資訊：

一是PCL的icp裡的transformation estimation是基於SVD的， SVD是奇異值分解，Singular Value Decomposition，後面我們會提到；
二是使用之前要至少set三個引數：
- setMaximumIterations，最大迭代次數，icp是一個迭代的方法，最多迭代這些次；
- setTransformationEpsilon，前一個變換矩陣和當前變換矩陣的差異小於閾值時，就認為已經收斂了，是一條收斂條件；
- setEuclideanFitnessEpsilon，還有一條收斂條件是均方誤差和小於閾值，停止迭代。

  /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. 
    * The transformation is estimated based on Singular Value Decomposition (SVD).
    *
    * The algorithm has several termination criteria:
    *
    * <li>Number of iterations has reached the maximum user imposed number of iterations (via \ref setMaximumIterations)</li>
    * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
    * <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
    * </ol>
    * Usage example:
    *  ...
    * \author Radu B. Rusu, Michael Dixon
**/

執行上面的程式碼就能得到兩個點雲配準的結果了，先不管結果好不好，看看他內部做了什麼，實際配准演算法都在aign裡實現。icp.h裡看到IterativeClosestPoint類是Registration的子類，icp.h給出了icp類的定義，而align的具體實現在上面的registration資料夾下的impl下的icp.hpp裡，都說align with initial guess，如果可以從一個好的初始猜想變換矩陣開始迭代，那麼演算法將會在比較少的迭代之後就收斂，配準結果也較好，當像我們這裡沒有指定初始guess時，就預設使用單位陣Matrix4::Identity() ，迭代部分就像下面這樣：

// Repeat until convergence
  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_);

    //...

    size_t cnt = correspondences_->size ();
    // Check whether we have enough correspondences
    if (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;
    }

    // Estimate the transform
    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_;

    converged_ = static_cast<bool> ((*convergence_criteria_));
  }
  while (!converged_);

這裡的 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);做了最重要的估計變換矩陣，

實際是用pcl::registration::TransformationEstimationSVD類來實現的，這裡至少有兩個資訊量，

第一，SVD奇異值分解在pcl裡是直接呼叫Eigen內部的Umeyama實現的，我看了一眼，太過數學，暫時跳過，在另一篇部落格中提過，Eigen是專門處理矩陣運算的庫，pcl用的3rdParty之一，pcl用了很多第三方，這也是為什麼當初配環境如此痛苦的原因之一，畢竟pcl這個庫最開始只是一個德國博士的畢業論文順手寫出來的；另外還可以看到，這裡的實現除了用SVD還有另一種方法，else裡提到的是先算兩個點雲的3D Centeroid, 再getTransformationFromCorrelation. 後面我們再來提這種思路。
第二， final_transformation = current_transformation * final_transformation, 這和KinFu那篇論文提到的“變換矩陣不斷疊加，最終得到唯一的全域性攝像頭位置（global camera pose）”是一致的。

template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
    ConstCloudIterator<PointSource>& source_it,
    ConstCloudIterator<PointTarget>& target_it,
    Matrix4 &transformation_matrix) const
{
  // Convert to Eigen format
  const int npts = static_cast <int> (source_it.size ());

  if (use_umeyama_)
  {
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);

    for (int i = 0; i < npts; ++i)
    {
      cloud_src (0, i) = source_it->x;
      cloud_src (1, i) = source_it->y;
      cloud_src (2, i) = source_it->z;
      ++source_it;

      cloud_tgt (0, i) = target_it->x;
      cloud_tgt (1, i) = target_it->y;
      cloud_tgt (2, i) = target_it->z;
      ++target_it;
    }
    
    // Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
    transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
  }
  else
  {
    source_it.reset (); target_it.reset ();
    // <cloud_src,cloud_src> is the source dataset
    transformation_matrix.setIdentity ();

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    // Estimate the centroids of source, target
    compute3DCentroid (source_it, centroid_src);
    compute3DCentroid (target_it, centroid_tgt);
    source_it.reset (); target_it.reset ();

    // Subtract the centroids from source, target
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
    demeanPointCloud (source_it, centroid_src, cloud_src_demean);
    demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);

    getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
  }
}

到這裡，pcl的演算法基本上也捋清楚了，我們大概總結一下：首先icp是一步一步迭代得到較好的結果，解的過程其實是一個優化問題，並不能達到絕對正解。這個過程中求兩個點雲之間變換矩陣是最重要的，PCL裡是用奇異值分解SVD實現的。