PCL学习笔记二：Registration (ICP算法)_"namespace \"pcl::registration\"没有成员\"transformati

先上例子, 下面是用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实现的。

PCL里有很多ICP可以用

pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >

is an ICP variant that implements the generalized iterative closest point algorithm as described by Alex Segal et al.

pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >

provides a base implementation of the Iterative Closest Point algorithm. 

pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >

is a special case of  IterativeClosestPoint , that uses a transformation estimated based on Point to Plane distances by default.

pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar >

 is an ICP variant that uses Levenberg-Marquardt optimization backend.

pcl::registration::IncrementalICP< PointT, Scalar >

This class provides a way to register a stream of clouds where each cloud will be aligned to the previous cloud.