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 convergencedo{// Get blob data if neededPCLPointCloud2::Ptr input_transformed_blob;if (need_source_blob_){input_transformed_blob.reset (new PCLPointCloud2);toPCLPointCloud2 (*input_transformed, *input_transformed_blob);}// Save the previously estimated transformationprevious_transformation_ = transformation_;// Set the source each iteration, to ensure the dirty flag is updatedcorrespondence_estimation_->setInputSource (input_transformed);if (correspondence_estimation_->requiresSourceNormals ())correspondence_estimation_->setSourceNormals (input_transformed_blob);// Estimate correspondencesif (use_reciprocal_correspondence_)correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);elsecorrespondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);//...size_t cnt = correspondences_->size ();// Check whether we have enough correspondencesif (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 transformtransformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);// Tranform the datatransformCloud (*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_);做了最重要的估计变换矩阵，

• 第一，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 formatconst 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 datasettransformation_matrix.setIdentity ();Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;// Estimate the centroids of source, targetcompute3DCentroid (source_it, centroid_src);compute3DCentroid (target_it, centroid_tgt);source_it.reset (); target_it.reset ();// Subtract the centroids from source, targetEigen::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::registration::IncrementalICP< PointT, Scalar >