文章目录

1.PCL-ICP代码框架
2. pcl::IterativeClosestPoint类格
3. pcl::Registration::align()详解
- 3.1 pcl::Registration::initCompute()详解
- 3.2 pcl::IterativeClosestPoint::computeTransformation()详解
- - 3.2.1 收敛状态简介
  - 3.2.2 computeTransformation代码解析
  - 3.2.3 pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()代码解析

1.PCL-ICP代码框架

话不多说，直接上代码

    #include <pcl/registration/icp.h>#include <pcl/point_types.h>pc_xyz::Ptr result = boost::make_shared<pc_xyz>();pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setMaxCorrespondenceDistance(max_correspondence_dist_);// 设置corr_dist_threshold_icp.setMaximumIterations(max_iterations_); //设置max_iterations_icp.setTransformationEpsilon(1e-6); //设置transformation_epsilon_icp.setEuclideanFitnessEpsilon(1e-2); // 设置euclidean_fitness_epsilon_icp.setInputSource(local_map_xyz);//设置input_ ，source_cloud_updated_ = true;icp.setInputTarget(global_map_xyz);//设置target_， target_cloud_updated_ = true;if (do_ransac_outlier_rejection_) {icp.setRANSACOutlierRejectionThreshold(ransac_outlier_rejection_threshold_);icp.setRANSACIterations(ransac_iterations_);}icp.align(*result);//主要匹配函数is_converged = icp.hasConverged();fitness = icp.getFitnessScore();correction = icp.getFinalTransformation();

2. pcl::IterativeClosestPoint类格

#mermaid-svg-B8yhEWvU7EJKfxak {font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-B8yhEWvU7EJKfxak .error-icon{fill:#552222;}#mermaid-svg-B8yhEWvU7EJKfxak .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-B8yhEWvU7EJKfxak .edge-thickness-normal{stroke-width:2px;}#mermaid-svg-B8yhEWvU7EJKfxak .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-B8yhEWvU7EJKfxak .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-B8yhEWvU7EJKfxak .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-B8yhEWvU7EJKfxak .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-B8yhEWvU7EJKfxak .marker{fill:#333333;stroke:#333333;}#mermaid-svg-B8yhEWvU7EJKfxak .marker.cross{stroke:#333333;}#mermaid-svg-B8yhEWvU7EJKfxak svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-B8yhEWvU7EJKfxak .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-B8yhEWvU7EJKfxak .cluster-label text{fill:#333;}#mermaid-svg-B8yhEWvU7EJKfxak .cluster-label span{color:#333;}#mermaid-svg-B8yhEWvU7EJKfxak .label text,#mermaid-svg-B8yhEWvU7EJKfxak span{fill:#333;color:#333;}#mermaid-svg-B8yhEWvU7EJKfxak .node rect,#mermaid-svg-B8yhEWvU7EJKfxak .node circle,#mermaid-svg-B8yhEWvU7EJKfxak .node ellipse,#mermaid-svg-B8yhEWvU7EJKfxak .node polygon,#mermaid-svg-B8yhEWvU7EJKfxak .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-B8yhEWvU7EJKfxak .node .label{text-align:center;}#mermaid-svg-B8yhEWvU7EJKfxak .node.clickable{cursor:pointer;}#mermaid-svg-B8yhEWvU7EJKfxak .arrowheadPath{fill:#333333;}#mermaid-svg-B8yhEWvU7EJKfxak .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-B8yhEWvU7EJKfxak .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-B8yhEWvU7EJKfxak .edgeLabel{background-color:#e8e8e8;text-align:center;}#mermaid-svg-B8yhEWvU7EJKfxak .edgeLabel rect{opacity:0.5;background-color:#e8e8e8;fill:#e8e8e8;}#mermaid-svg-B8yhEWvU7EJKfxak .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-B8yhEWvU7EJKfxak .cluster text{fill:#333;}#mermaid-svg-B8yhEWvU7EJKfxak .cluster span{color:#333;}#mermaid-svg-B8yhEWvU7EJKfxak div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-B8yhEWvU7EJKfxak :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;}

pcl::IterativeClosestPoint

pcl::Registration

pcl::PCLBase

3. pcl::Registration::align()详解

第1小节中的

  icp.align(*result);//主要匹配函数

对应的为pcl/registtation/impl/registation.hpp中的

template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
{align (output, Matrix4::Identity ());
}

其调用pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess) 函数。
ICP的核心内容也就是该函数，接下来我们对其进行详细解释：

template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
{if (!initCompute ()) //初始化kd树，及点云索引return;// Resize the output datasetif (output.points.size () != indices_->size ())output.points.resize (indices_->size ());// Copy the headeroutput.header   = input_->header;// Check if the output will be computed for all points or only a subsetif (indices_->size () != input_->points.size ()){output.width    = static_cast<uint32_t> (indices_->size ());output.height   = 1;}else{output.width    = static_cast<uint32_t> (input_->width);output.height   = input_->height;}output.is_dense = input_->is_dense;// Copy the point data to outputfor (size_t i = 0; i < indices_->size (); ++i)output.points[i] = input_->points[(*indices_)[i]];//将原始点云复制到output中// Set the internal point representation of choice unless otherwise notedif (point_representation_ && !force_no_recompute_) tree_->setPointRepresentation (point_representation_);// Perform the actual transformation computationconverged_ = 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 // transformationfor (size_t i = 0; i < indices_->size (); ++i)output.points[i].data[3] = 1.0;computeTransformation (output, guess);//求解旋转矩阵，动态绑定到pcl::IterativeClosestPoint::computeTransformation()deinitCompute ();// return (true); 直接返回true
}

3.1 pcl::Registration::initCompute()详解

initCompute用于初始化kd树，及点云索引

template <typename PointSource, typename PointTarget, typename Scalar>
bool
Registration<PointSource, PointTarget, Scalar>::initCompute()
{if (!target_) {PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",getClassName().c_str());return (false);}// Only update target kd-tree if a new target cloud was setif (target_cloud_updated_ && !force_no_recompute_) {//force_no_recompute_默认初始化为false，因此第一次运行时，进入该if判断tree_->setInputCloud(target_);//用目标点云初始化kd树target_cloud_updated_ = false;}// Update the correspondence estimationif (correspondence_estimation_) {correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_);//初始化correspondence_estimation_，包括    tree_ = tree;    force_no_recompute_ = force_no_recompute;    target_cloud_updated_ = true;correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,force_no_recompute_reciprocal_);}// Note: we /cannot/ update the search method on all correspondence rejectors, because// we know nothing about them. If they should be cached, they must be cached// individually.return (PCLBase<PointSource>::initCompute());//初始化与input_的size对应的indices_
}

3.2 pcl::IterativeClosestPoint::computeTransformation()详解

3.2.1 收敛状态简介

        enum ConvergenceState{CONVERGENCE_CRITERIA_NOT_CONVERGED,//默认值，converged_ = false;CONVERGENCE_CRITERIA_ITERATIONS,//迭代结果为达到迭代次数，此时若设置failure_after_max_iter_ = true，就会导致converged_ = false;默认状态是failure_after_max_iter_ = false，因此该情况下超过迭代次数会判断converged_ = trueCONVERGENCE_CRITERIA_TRANSFORM,//平移距离的平方和满足transformation_epsilon_要求，则判断CONVERGENCE_CRITERIA_TRANSFORM，  converged_ = trueCONVERGENCE_CRITERIA_ABS_MSE,//两次迭代的绝对距离差满足收敛要求，converged_ = trueCONVERGENCE_CRITERIA_REL_MSE,//两次迭代的相对距离差满足收敛要求，converged_ = trueCONVERGENCE_CRITERIA_NO_CORRESPONDENCES//迭代结果为对应点对太少，converged_ = false;};

3.2.2 computeTransformation代码解析

pcl::IterativeClosestPoint::computeTransformation() 位于pcl/registration/impl/icp.hpp文件中：

template <typename PointSource, typename PointTarget, typename Scalar>
void
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_ = 0;converged_ = false;// Initialise final transformation to the guessed onefinal_transformation_ = guess;// If the guessed transformation is non identityif (guess != Matrix4::Identity()) {input_transformed->resize(input_->size());// Apply guessed transformation prior to search for neighbourstransformCloud(*input_, *input_transformed, guess);}else*input_transformed = *input_;transformation_ = Matrix4::Identity();// Make blobs if necessarydetermineRequiredBlobData();// 未指定correspondence_rejectors_ 以及点云都没有法向量的话，need_source_blob_以及need_target_blob_ 都是false，即不要求处理“二进制长对象（blob）”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 codecorrespondence_estimation_->setInputTarget(target_); //设置CorrespondenceEstimationBase中的target_ ，target_cloud_updated_ = true；if (correspondence_estimation_->requiresTargetNormals())correspondence_estimation_->setTargetNormals(target_blob);// Correspondence Rejectors need a binary blobfor (std::size_t i = 0; 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_);//设置DefaultConvergenceCriteria 中的max_iterations_convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);//设置DefaultConvergenceCriteria 中的mse_threshold_relative_convergence_criteria_->setTranslationThreshold(transformation_epsilon_);//设置DefaultConvergenceCriteria 中的translation_threshold_if (transformation_rotation_epsilon_ > 0)convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);elseconvergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);//设置DefaultConvergenceCriteria 中的rotation_threshold_// Repeat until convergence//以下进入icp迭代解算do {// 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_);//基于kd树最近邻查找最近点。// if (correspondence_rejectors_.empty ())CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));for (std::size_t i = 0; 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 iterationif (i < correspondence_rejectors_.size() - 1)*temp_correspondences = *correspondences_;}std::size_t cnt = correspondences_->size();// Check whether we have enough correspondencesif (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;//若对应点对数太少，则直接终止icp，收敛状态为false}// Estimate the transformtransformation_estimation_->estimateRigidTransformation(*input_transformed, *target_, *correspondences_, transformation_);//进行svd位姿解算// Transform the datatransformCloud(*input_transformed, *input_transformed, transformation_);// Obtain the final transformationfinal_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_));//这里显式类型转换调用了pcl::registration::ConvergenceCriteria中对bool的重载，该重载中调用了pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged（）函数} while (convergence_criteria_->getConvergenceState() ==pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);// Transform the input cloud using the final transformationPCL_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_(0, 0),final_transformation_(0, 1),final_transformation_(0, 2),final_transformation_(0, 3),final_transformation_(1, 0),final_transformation_(1, 1),final_transformation_(1, 2),final_transformation_(1, 3),final_transformation_(2, 0),final_transformation_(2, 1),final_transformation_(2, 2),final_transformation_(2, 3),final_transformation_(3, 0),final_transformation_(3, 1),final_transformation_(3, 2),final_transformation_(3, 3));//打印最终的变换矩阵// Copy all the valuesoutput = *input_;// Transform the XYZ + normalstransformCloud(*input_, output, final_transformation_);//进行坐标转换获取最终结果output
}

3.2.3 pcl::registration::DefaultConvergenceCriteria::hasConverged ()代码解析

template <typename Scalar> bool
pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()
{convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_);// 1. Number of iterations has reached the maximum user imposed number of iterationsif (iterations_ >= max_iterations_){if (failure_after_max_iter_)return (false);else{convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;return (true);}return (failure_after_max_iter_ ? false : true);}// 2. The epsilon (difference) between the previous transformation and the current estimated transformationdouble cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);//罗德里格斯公式double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) +transformation_.coeff (1, 3) * transformation_.coeff (1, 3) +transformation_.coeff (2, 3) * transformation_.coeff (2, 3);//平移二范数PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave %f rotation (cosine) and %f translation.\n", cos_angle, translation_sqr);if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_)//平移距离的二范数满足要求则判断CONVERGENCE_CRITERIA_TRANSFORM{if (iterations_similar_transforms_ < max_iterations_similar_transforms_){// Increment the number of transforms that the thresholds are allowed to be similar++iterations_similar_transforms_;return (false);}else{iterations_similar_transforms_ = 0;convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM;return (true);}}correspondences_cur_mse_ = calculateMSE (correspondences_);//计算平均距离，单位为mPCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: %f / %f.\n", correspondences_prev_mse_, correspondences_cur_mse_);// 3. The relative sum of Euclidean squared errors is smaller than a user defined threshold// Absoluteif (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_)//两次迭代的绝对距离差满足收敛要求{if (iterations_similar_transforms_ < max_iterations_similar_transforms_){// Increment the number of transforms that the thresholds are allowed to be similar++iterations_similar_transforms_;return (false);}else{iterations_similar_transforms_ = 0;convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE;return (true);}}// Relativeif (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_)//两次迭代的相对距离差满足收敛要求{if (iterations_similar_transforms_ < max_iterations_similar_transforms_){// Increment the number of transforms that the thresholds are allowed to be similar++iterations_similar_transforms_;return (false);}else{iterations_similar_transforms_ = 0;convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE;return (true);}}correspondences_prev_mse_ = correspondences_cur_mse_;return (false);
}

PCL-ICP(IterativeClosestPoint)源码解析相关推荐

点云配准2：icp算法在PCL1.10.0上的实现+源码解析
目录本文最后实现的配准实例点云配准系列准备程序结构主程序 1.为什么要降采样 2.体素降采样原理 3.点云更新 icp 配准前的参数设置 icp配准算法内部对应点对确定(determine ...
多激光雷达外参标定算法与源码解析(一):基于BLAM的建图模块
前言原理文字介绍略微简介,但是在代码注释中非常详细.我相信在代码中学习原理才能理解更加通透. 代码参考自livox sdk: gitcode 一.算法原理二.源码解析函数流:main->B ...
loam源码解析5 ： laserOdometry（三）
transformMaintenance.cpp解析八.位姿估计 1. 雅可比计算 2. 矩阵求解 3. 退化问题分析 4. 姿态更新 5. 坐标转换 loam源码地址: https://githu ...
谷歌BERT预训练源码解析（二）：模型构建
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...
谷歌BERT预训练源码解析（三）：训练过程
目录前言源码解析主函数自定义模型遮蔽词预测下一句预测规范化数据集前言本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...
谷歌BERT预训练源码解析（一）：训练数据生成
目录预训练源码结构简介输入输出源码解析参数主函数创建训练实例下一句预测&实例生成随机遮蔽输出结果一览预训练源码结构简介关于BERT,简单来说,它是一个基于Transfo ...
Gin源码解析和例子——中间件（middleware）
在<Gin源码解析和例子--路由>一文中,我们已经初识中间件.本文将继续探讨这个技术.(转载请指明出于breaksoftware的csdn博客) Gin的中间件,本质是一个匿名回调函数.这 ...
Colly源码解析——结合例子分析底层实现
通过<Colly源码解析--框架>分析,我们可以知道Colly执行的主要流程.本文将结合http://go-colly.org上的例子分析一些高级设置的底层实现.(转载请指明出于break ...
libev源码解析——定时器监视器和组织形式
我们先看下定时器监视器的数据结构.(转载请指明出于breaksoftware的csdn博客) /* invoked after a specific time, repeatable (based o ...
libev源码解析——定时器原理
本文将回答<libev源码解析--I/O模型>中抛出的两个问题.(转载请指明出于breaksoftware的csdn博客) 对于问题1:为什么backend_poll函数需要指定超时?我们 ...

PCL-ICP(IterativeClosestPoint)源码解析