[PCL]5 ICP算法进行点云匹配

[PCL]5 ICP算法进行点云匹配

上一篇：http://www.cnblogs.com/yhlx125/p/4924283.html截图了一些ICP算法进行点云匹配的类图。

但是将对应点剔除这块和ICP算法的关系还是没有理解。

RANSAC算法可以实现点云剔除，但是ICP算法通过稳健性的算法实现匹配，似乎不进行对应点剔除。

是不是把全局的点云匹配方法和局部点云匹配方法搞混了？

ICP算法可以通过三种方式处理噪声、部分重叠的问题：剔除、权重、Trimmed方法和稳健估计方法。

下面分析一下PCL中关于ICP算法的实现。

首先是IterativeClosestPoint模板类继承自Registration模板类。

查看icp.h中的定义：

template <typename PointSource, typename PointTarget, typename Scalar = float>

  class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>

查看registration.h中的定义：

 template <typename PointSource, typename PointTarget, typename Scalar = float>

  class Registration : public PCLBase<PointSource>

同样的IterativeClosestPointNonLinear 继承自IterativeClosestPoint，查看icp_nl.h

template <typename PointSource, typename PointTarget, typename Scalar = float> class IterativeClosestPointNonLinear : public IterativeClosestPoint<PointSource, PointTarget, Scalar>

区别在于ICP算法的求解方式不同，非线性求解采用的是LM算法：http://www.cnblogs.com/yhlx125/p/4955337.html

下面重点说明IterativeClosestPoint模板类。

匹配的关键方法Align在Registration中实现。

 template <typename PointSource, typename PointTarget, typename Scalar> inline void

 pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)

 {

   if (!initCompute ())

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

   }

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

   for (size_t i = ; i < indices_->size (); ++i)

     output.points[i].data[] = 1.0;

   computeTransformation (output, guess);



   deinitCompute ();

 }

重点关注computeTransformation虚方法。

显然IterativeClosestPoint重载了基类这个方法。

virtual void computeTransformation (PointCloudSource &output, const Matrix4& guess) = ;

代码如下：

 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>

   PointCloudSourcePtr input_transformed (new PointCloudSource);

   nr_iterations_ = ;

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

   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 = ; 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_);

   convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);

   convergence_criteria_->setTranslationThreshold (transformation_epsilon_);

   convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);

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



     //if (correspondence_rejectors_.empty ())

     CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));

     for (size_t i = ; 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 () - )

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

     }

     // 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_;

     // 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_ (, ), final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ),

       final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ),

       final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ),

       final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ), final_transformation_ (, ));

   // Copy all the values

   output = *input_;

   // Transform the XYZ + normals

   transformCloud (*input_, output, final_transformation_);

 }

该方法的主体是一个do-while循环。

这里要说三个内容：correspondence_estimation_ 、correspondence_rejectors_ 和 convergence_criteria_ 。

三个变量的作用分别是查找最近点，剔除错误的对应点，收敛准则。

因为是do-while循环，因此收敛准则的作用是跳出循环。

transformation_estimation_是求解ICP的具体算法：

transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);

查看类图可以知道包括SVD奇异值分解算法，查看transformation_estimation_svd.hpp中的TransformationEstimationSVD类的estimateRigidTransformation 方法：

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

其中通过调用下面的代码实现了SVD求解，具体方法内部实现时通过Eigen3实现的。

需要具体查看，可以借鉴。

transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);