但是將對應點剔除這塊和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> 

下面重點說明IterativeClosestPoint模板類。匹配的關鍵方法Align在Registration中實現。

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

 2 pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
 3 {
 4   if (!initCompute ()) 
 5     return;
 6 
 7   // Resize the output dataset
 8   if (output.points.size () != indices_->size ())
 9     output.points.resize (indices_->size ());
10   // Copy the header
11   output.header   = input_->header;
12   // Check if the output will be computed for all points or only a subset
13   if (indices_->size () != input_->points.size ())
14   {
15     output.width    = static_cast<uint32_t> (indices_->size ());
16     output.height   = 1;
17   }
18   else
19   {
20     output.width    = static_cast<uint32_t> (input_->width);
21     output.height   = input_->height;
22   }
23   output.is_dense = input_->is_dense;
24 
25   // Copy the point data to output
26   for (size_t i = 0; i < indices_->size (); ++i)
27     output.points[i] = input_->points[(*indices_)[i]];
28 
29   // Set the internal point representation of choice unless otherwise noted
30   if (point_representation_ && !force_no_recompute_) 
31     tree_->setPointRepresentation (point_representation_);
32 
33   // Perform the actual transformation computation
34   converged_ = false;
35   final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
36 
37   // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 
38   // transformation
39   for (size_t i = 0; i < indices_->size (); ++i)
40     output.points[i].data[3] = 1.0;
41 
42   computeTransformation (output, guess);
43 
44   deinitCompute ();
45 }

重點關注computeTransformation虛方法。顯然IterativeClosestPoint過載了基類這個方法。

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

程式碼如下：

  1 template <typename PointSource, typename PointTarget, typename Scalar> void
  2 pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
  3     PointCloudSource &output, const Matrix4 &guess)
  4 {
  5   // Point cloud containing the correspondences of each point in <input, indices>
  6   PointCloudSourcePtr input_transformed (new PointCloudSource);
  7 
  8   nr_iterations_ = 0;
  9   converged_ = false;
 10 
 11   // Initialise final transformation to the guessed one
 12   final_transformation_ = guess;
 13 
 14   // If the guessed transformation is non identity
 15   if (guess != Matrix4::Identity ())
 16   {
 17     input_transformed->resize (input_->size ());
 18      // Apply guessed transformation prior to search for neighbours
 19     transformCloud (*input_, *input_transformed, guess);
 20   }
 21   else
 22     *input_transformed = *input_;
 23  
 24   transformation_ = Matrix4::Identity ();
 25 
 26   // Make blobs if necessary
 27   determineRequiredBlobData ();
 28   PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
 29   if (need_target_blob_)
 30     pcl::toPCLPointCloud2 (*target_, *target_blob);
 31 
 32   // Pass in the default target for the Correspondence Estimation/Rejection code
 33   correspondence_estimation_->setInputTarget (target_);
 34   if (correspondence_estimation_->requiresTargetNormals ())
 35     correspondence_estimation_->setTargetNormals (target_blob);
 36   // Correspondence Rejectors need a binary blob
 37   for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
 38   {
 39     registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
 40     if (rej->requiresTargetPoints ())
 41       rej->setTargetPoints (target_blob);
 42     if (rej->requiresTargetNormals () && target_has_normals_)
 43       rej->setTargetNormals (target_blob);
 44   }
 45 
 46   convergence_criteria_->setMaximumIterations (max_iterations_);
 47   convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
 48   convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
 49   convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
 50 
 51   // Repeat until convergence
 52   do
 53   {
 54     // Get blob data if needed
 55     PCLPointCloud2::Ptr input_transformed_blob;
 56     if (need_source_blob_)
 57     {
 58       input_transformed_blob.reset (new PCLPointCloud2);
 59       toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
 60     }
 61     // Save the previously estimated transformation
 62     previous_transformation_ = transformation_;
 63 
 64     // Set the source each iteration, to ensure the dirty flag is updated
 65     correspondence_estimation_->setInputSource (input_transformed);
 66     if (correspondence_estimation_->requiresSourceNormals ())
 67       correspondence_estimation_->setSourceNormals (input_transformed_blob);
 68     // Estimate correspondences
 69     if (use_reciprocal_correspondence_)
 70       correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
 71     else
 72       correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
 73 
 74     //if (correspondence_rejectors_.empty ())
 75     CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
 76     for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
 77     {
 78       registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
 79       PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
 80       if (rej->requiresSourcePoints ())
 81         rej->setSourcePoints (input_transformed_blob);
 82       if (rej->requiresSourceNormals () && source_has_normals_)
 83         rej->setSourceNormals (input_transformed_blob);
 84       rej->setInputCorrespondences (temp_correspondences);
 85       rej->getCorrespondences (*correspondences_);
 86       // Modify input for the next iteration
 87       if (i < correspondence_rejectors_.size () - 1)
 88         *temp_correspondences = *correspondences_;
 89     }
 90 
 91     size_t cnt = correspondences_->size ();
 92     // Check whether we have enough correspondences
 93     if (static_cast<int> (cnt) < min_number_correspondences_)
 94     {
 95       PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
 96       convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
 97       converged_ = false;
 98       break;
 99     }
100 
101     // Estimate the transform
102     transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
103 
104     // Tranform the data
105     transformCloud (*input_transformed, *input_transformed, transformation_);
106 
107     // Obtain the final transformation    
108     final_transformation_ = transformation_ * final_transformation_;
109 
110     ++nr_iterations_;
111 
112     // Update the vizualization of icp convergence
113     //if (update_visualizer_ != 0)
114     //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );
115 
116     converged_ = static_cast<bool> ((*convergence_criteria_));
117   }
118   while (!converged_);
119 
120   // Transform the input cloud using the final transformation
121   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", 
122       final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
123       final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
124       final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
125       final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
126 
127   // Copy all the values
128   output = *input_;
129   // Transform the XYZ + normals
130   transformCloud (*input_, output, final_transformation_);
131 }

該方法的主體是一個do-while迴圈。這裡要說三個內容：correspondence_estimation_ 、correspondence_rejectors_ 和 convergence_criteria_ 。

三個變數的作用分別是查詢最近點，剔除錯誤的對應點，收斂準則。因為是do-while迴圈，因此收斂準則的作用是跳出迴圈。

transformation_estimation_是求解ICP的具體演算法：

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