上一篇：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);