上一篇：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中實現。
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);