【PCL】—— 点云配准ICP(Iterative Closest Point)算法

文章目录

• 数学原理
• • 问题定义
  • 计算平移
  • 计算旋转
• 案例实现
• 参考

​     由于三维扫描仪设备受到测量方式和被测物体形状的条件限制，一次扫描往往只能获取到局部的点云信息，进而需要进行多次扫描，然后每次扫描时得到的点云都有独立的坐标系，不可以直接进行拼接。在逆向工程、计算机视觉、文物数字化等领域中，由于点云的不完整、旋转错位、平移错位等，使得要得到完整点云就需要对多个局部点云进行配准。为了得到被测物体的完整数据模型,需要确定一个合适的坐标变换 ，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化等操作，这就是点云数据的配准。

​     点云配准步骤上可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两个阶段。

​     粗配准是指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。

​     精配准是指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。所以需要各种粗配准技术为ICP算法提供较好的位置，在迭代过程中确立正确对应点集能避免迭代陷入局部极值，决定了算法的收敛速度和最终的配准精度。

​     比较常见的一种配准算法是迭代最近点算法(Iterative Closest Point, ICP)。ICP算法可以基于四元数求解，也可以基于奇异值分解（SVD）求解，本文主要介绍基于奇异值分解的ICP算法。

ICP算法原理：给定一个参考点集$P$和一个数据点集$Q$（在给定的初始估计$R$，$t$），算法为$Q$中的每个点寻找$P$中对应的最近点，形成匹配点对。然后，将所有匹配点对的欧氏距离之和作为待求解的目标函数，利用奇异值分解求出$R$和$t$以使目标函数最小，根据$R$, $t$转换得到新的 Q′ Q’Q′，并再次找到对应的点对，如此迭代。

缺点：需要剔除噪声点（距离过大的点对或包含边界点的点对）。基于点对的配准不包括局部形状信息。在每次迭代中搜索最近点非常耗时。计算可能陷入局部最优。

​     一般来说，ICP可以分为以下四个阶段

1. 对原始点云数据进行采样
2. 确定初始对应点集
3. 去除错误对应点集
4. 坐标变换的求解

数学原理

问题定义

本部分结合了网上的一些资料以及自己的一些理解，可能不一定正确，部分步骤有所省略，具体可参考文末参考文献。

定义两个点云集合 P = { p 1, p 2,…, p n} P={\{p_1,p_2,…,p_n\}}P={p1​,p2​,…,pn​}, Q = { q 1, q 2,…, q n} Q={\{q_1,q_2,…,q_n\}}Q={q1​,q2​,…,qn​}，其中$P$为参考点云集合，$Q$为数据点云集合。点云的配准即是需要将$Q$通过一系列的旋转与平移得到目标$P$，实际上两个点云集合不会完全相同，所以往往无法得到准确的$R$和$t$使二者完全重合，但可以使变换后的点云$Q$尽可能靠近$P$，因此可以将问题描述为:
E(R,t)= arg min⁡R∈SO(d),t∈ R d∑ i=1n wi ∥ R q i+t− p i∥2 E( R,t) = {\argmin\limits_{R \in SO(d),t\in {\Reals^d}}}\sum\limits_{i = 1}^n {{w_i}{{\left\| {R{q_i} + t – {p_i}} \right\|}^2}} E(R,t)=R∈SO(d),t∈Rdargmin​i=1∑n​wi​∥Rqi​+t−pi​∥2

即构建最小二乘问题，将之转化为目标函数的优化问题，求以使以下目标函数误差的平方和达到极小时的$R$和$t$。其中 wi w_iwi​代表每个点的权重。$R$ 和 $t$是我们所要求的旋转矩阵和平移向量。其中，$d$ 表示 $x$ 和 $y$的维度，通常情况下对于三维点云就是 $d$ = 3，对于二维点云就是 $d$ = 2。

计算平移

假设$R$是常量，则先求平移向量$t$ ，令∂ E ∂ t= 0\frac{{\partial E}}{{\partial t}} = 0∂t∂E​=0 , 可得目标函数 ∂E∂t = ∑ i=1n 2 wi( R qi+ t − pi)=2t (∑ i=1n w i)+2R (∑ i=1n w i q i)−2 (∑ i=1n wipi )=0\begin{array}{c}\frac{{\partial E}}{{\partial t}} = \sum\limits_{i = 1}^n {2{w_i}(R{q_i} + t – {p_i})} \\ = 2t\left( {\sum\limits_{i = 1}^n {{w_i}} } \right) + 2R\left( {\sum\limits_{i = 1}^n {{w_i}} {q_i}} \right) – 2\left( {\sum\limits_{i = 1}^n {{w_i}{p_i}} } \right) = 0\end{array} ∂t∂E​=i=1∑n​2wi​(Rqi​+t−pi​)=2t(i=1∑n​wi​)+2R(i=1∑n​wi​qi​)−2(i=1∑n​wi​pi​)=0​

令 $p$ 的质心 μp= 1n⋅ ∑ i = 1npi {\mu _p} = \frac{1}{n} \cdot \sum\limits_{i = 1}^n {{p_i}}μp​=n1​⋅i=1∑n​pi​, $q$ 的质心 μq= 1n⋅ ∑ i = 1nqi {\mu _q} = \frac{1}{n} \cdot \sum\limits_{i = 1}^n {{q_i}}μq​=n1​⋅i=1∑n​qi​.则可得 μp− R μq= t{\mu _p} – R{\mu _q} = tμp​−Rμq​=t

将以上关系替换回原函数
E(R,t)= arg min⁡R∈SO(d),t∈ R d∑ i=1n wi ∥ R q i+t− p i∥2 = arg min⁡R∈SO(d),t∈ R d∑ i=1n wi ∥ R q i+ μ p−R μ q− p i∥2 = arg min⁡R∈SO(d),t∈ R d∑ i=1n wi ∥ R( q i− μ q)+ μ p− p i∥2 = arg min⁡R∈SO(d),t∈ R d∑ i=1n wi ∥p′i−R q′i∥2 \begin{array}{c}E(R,t) = {\argmin\limits_{R \in SO(d),t\in {\Reals^d}}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {R{q_i} + t – {p_i}} \right\|}^2}} \\ = {\argmin\limits_{R \in SO(d),t\in {\Reals^d}}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {R{q_i} + {\mu _p} – R{\mu _q} – {p_i}} \right\|}^2}} \\ = {\argmin\limits_{R \in SO(d),t\in {\Reals^d}}}\sum\limits_{i = 1}^n {{w_i}{{\left\| {R({q_i} – {\mu _q}) + {\mu _p} – {p_i}} \right\|}^2}} \\ = {\argmin\limits_{R \in SO(d),t\in {\Reals^d}}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {{{p’}_i} – R{{q’}_i}} \right\|}^2}} \end{array} E(R,t)=R∈SO(d),t∈Rdargmin​i=1∑n​wi​∥Rqi​+t−pi​∥2=R∈SO(d),t∈Rdargmin​i=1∑n​wi​∥Rqi​+μp​−Rμq​−pi​∥2=R∈SO(d),t∈Rdargmin​i=1∑n​wi​∥R(qi​−μq​)+μp​−pi​∥2=R∈SO(d),t∈Rdargmin​i=1∑n​wi​∥p′i​−Rq′i​∥2​
这里， pi′, qi′ p^′_i,q^′_ipi′​,qi′​ 分别是每个点云根据其质心坐标 μp, μq μ_p,μ_qμp​,μq​计算得到的去质心坐标：
q′i= q i− μ q, p′i= p i− μ p{{q’}_i} = {q_i} – {\mu _q},{{p’}_i} = {p_i} – {\mu _p} q′i​=qi​−μq​,p′i​=pi​−μp​

即求使得以下目标函数最小时，$R$的值
E(R,t)= arg min⁡R∈SO(d)∑ i=1n wi ∥p′i−R q′i∥2 E(R,t) = {\argmin\limits_{R \in SO(d)}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {{{p’}_i} – R{{q’}_i}} \right\|}^2}} E(R,t)=R∈SO(d)argmin​i=1∑n​wi​∥p′i​−Rq′i​∥2

计算旋转

将 ∑ i = 1n ∥p′i−R q′i∥2 \sum\limits_{i = 1}^n {{{\left\| {{{p’}_i} – R{{q’}_i}} \right\|}^2}}i=1∑n​∥p′i​−Rq′i​∥2展开即可得到 ∑ i=1n pi′T p i ′−2 pi′TR q i ′+ qi′T R TR q i ′\sum\limits_{i = 1}^n{p’_i}^T{p’_i} – 2{p’_i}^TR{q’_i} + {q’_i}^T{R^T}R{q’_i} i=1∑n​pi′​Tpi′​−2pi′​TRqi′​+qi′​TRTRqi′​
此时，我们发现，除了第二项以外，目标函数其他部分都与参数R无关，进而在求解极值时可忽略。进而我们将问题转换为求解使以下目标函数最小时，$R$的值：E(R,t)= arg min⁡R∈SO(d)∑ i=1n wi⋅ ( − 2) p′iTRq ′i E(R,t) = {\argmin\limits_{R \in SO(d)}} \sum\limits_{i = 1}^n {{w_i} \cdot \left( { – 2} \right){{p’}_i}^TR{{q’}_i}} E(R,t)=R∈SO(d)argmin​i=1∑n​wi​⋅(−2)p′i​TRq′i​= arg max⁡R∈SO(d)∑ i=1n wi p′iTRq ′i = {\argmax \limits_{R \in SO(d)}} \sum\limits_{i = 1}^n {{w_i}{{p’}_i}^TR{{q’}_i}} =R∈SO(d)argmax​i=1∑n​wi​p′i​TRq′i​

根据矩阵迹的性质，我们有以下结论： ∑ i=1n wi p′iTRq ′i =tr(W P TRQ)\sum\limits_{i = 1}^n {{w_i}{{p’}_i}^TR{{q’}_i}} = tr(W{P^T}RQ) i=1∑n​wi​p′i​TRq′i​=tr(WPTRQ)因为$$tr(AB)=tr(BA)$$所以tr(W P TRQ)=tr((W P T)RQ)=tr(RQW P T)tr(W{P^T}RQ) = tr((W{P^T})RQ) = tr(RQW{P^T}) tr(WPTRQ)=tr((WPT)RQ)=tr(RQWPT)定义“协方差”矩阵 S = Q W PT S = QW{P^T}S=QWPT对 $S$ 进行 SVD 分解：S=UΣ V TS = U\Sigma {V^T} S=UΣVT$S$为3×3矩阵，$\Sigma$为奇异值组成的对角矩阵， U , VT U,V^TU,VT为正交矩阵。

**定理：**对于任意一个正定矩阵 A AT AA^TAAT和正交矩阵$B$,都有tr(A A T)≥tr(BA A T)tr(AA^T)\ge tr(BAA^T) tr(AAT)≥tr(BAAT)

依据迹的性质，则可以得到旋转矩阵$R$
当 d e t ( V UT) = 1det(VU^T)=1det(VUT)=1时， V UT VU^TVUT为所求旋转矩阵
当 d e t ( V UT) = − 1det(VU^T)=-1det(VUT)=−1时， V UT VU^TVUT为反射矩阵，可以通过以下方式计算R=V [ 1 1 ⋱ ⋱ det⁡(V U T)] U TR = V\left[ {\begin{array}{ccccccccccccccc}1&{}&{}&{}&{}\\{}&1&{}&{}&{}\\{}&{}& \ddots &{}&{}\\{}&{}&{}& \ddots &{}\\{}&{}&{}&{}&{\det (V{U^T})}\end{array}} \right]{U^T} R=V ​1​1​⋱​⋱​det(VUT)​ ​UT

案例实现

官方教程——https://pcl.readthedocs.io/projects/tutorials/en/master/iterative_closest_point.html#iterative-closest-point

#include #include #include #include intmain(int argc, char **argv) {// 定义输入和输出点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);// 随机填充无序点云cloud_in->width = 5;cloud_in->height = 1;cloud_in->is_dense = false;cloud_in->points.resize(cloud_in->width * cloud_in->height);for (size_t i = 0; i < cloud_in->points.size(); ++i) {cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);}std::cout << "Saved " << cloud_in->points.size() << " data points to input:"<< std::endl;for (size_t i = 0; i < cloud_in->points.size(); ++i)std::cout << "" <<cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<cloud_in->points[i].z << std::endl;*cloud_out = *cloud_in;std::cout << "size:" << cloud_out->points.size() << std::endl;// 在点云上执行简单的刚性变换，将cloud_out中的x平移0.7f米，然后再次输出数据值。for (size_t i = 0; i < cloud_in->points.size(); ++i)cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;// 打印这些点std::cout << "Transformed " << cloud_in->points.size() << " data points:"<< std::endl;for (size_t i = 0; i < cloud_out->points.size(); ++i)std::cout << "" << cloud_out->points[i].x << " " <<cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;// 创建IterativeClosestPoint的实例// setInputSource将cloud_in作为输入点云// setInputTarget将平移后的cloud_out作为目标点云pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(cloud_in);icp.setInputTarget(cloud_out);// 创建一个 pcl::PointCloud实例 Final 对象,存储配准变换后的源点云,// 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话// （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp. hasConverged()= 1 (true),// 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。pcl::PointCloud<pcl::PointXYZ> Final;icp.align(Final);std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation();std::cout << matrix << std::endl;return (0);}

Saved 5 data points to input:0.352222 -0.151883 -0.106395-0.397406 -0.473106 0.292602-0.731898 0.667105 0.441304-0.734766 0.854581 -0.0361733-0.4607 -0.277468 -0.916762size:5Transformed 5 data points:1.05222 -0.151883 -0.1063950.302594 -0.473106 0.292602-0.0318983 0.667105 0.441304-0.0347655 0.854581 -0.03617330.2393 -0.277468 -0.916762has converged:1 score: 6.8559e-14 1 -7.93021e-09 -5.43139e-080.7-2.01282e-071 -4.93211e-08-8.8662e-08-2.45116e-081.85975e-071 -1.04308e-08 0001

交互式ICP配准
官方教程——https://pcl.readthedocs.io/projects/tutorials/en/latest/interactive_icp.html

该程序将加载点云并对其施加刚性变换。 之后，ICP算法会将变换后的点云与原始对齐。 每次用户按下“空格”，都会进行一次ICP迭代，并刷新查看器。

代码

#include #include #include #include #include #include #include  // TicToc typedef pcl::PointXYZ PointT;typedef pcl::PointCloud<PointT> PointCloudT;bool next_iteration = false;voidprint4x4Matrix (const Eigen::Matrix4d & matrix){printf ("Rotation matrix :\n");printf ("| %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));printf ("| %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));printf ("Translation vector :\n");printf ("t = \n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));}/** * 此函数是查看器的回调。 当查看器窗口位于顶部时，只要按任意键，就会调用此函数。 如果碰到“空格”； 将布尔值设置为true。 * @param event * @param nothing */voidkeyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, void* nothing){if (event.getKeySym () == "space" && event.keyDown ())next_iteration = true;}intmain (int argc,char* argv[]){// The point clouds we will be usingPointCloudT::Ptr cloud_in (new PointCloudT);// Original point cloudPointCloudT::Ptr cloud_tr (new PointCloudT);// Transformed point cloudPointCloudT::Ptr cloud_icp (new PointCloudT);// ICP output point cloud//我们检查程序的参数，设置初始ICP迭代的次数，然后尝试加载PCD文件。// Checking program argumentsif (argc < 2){printf ("Usage :\n");printf ("\t\t%s file.pcd number_of_ICP_iterations\n", argv[0]);PCL_ERROR ("Provide one pcd file.\n");return (-1);}int iterations = 1;// Default number of ICP iterationsif (argc > 2){// If the user passed the number of iteration as an argumentiterations = atoi (argv[2]);if (iterations < 1){PCL_ERROR ("Number of initial iterations must be >= 1\n");return (-1);}}pcl::console::TicToc time;time.tic ();if (pcl::io::loadPCDFile (argv[1], *cloud_in) < 0){PCL_ERROR ("Error loading cloud %s.\n", argv[1]);return (-1);}std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;// 我们使用刚性矩阵变换来变换原始点云。// cloud_in包含原始点云。// cloud_tr和cloud_icp包含平移/旋转的点云。// cloud_tr是我们将用于显示的备份（绿点云）。// Defining a rotation matrix and translation vectorEigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)double theta = M_PI / 8;// The angle of rotation in radianstransformation_matrix (0, 0) = std::cos (theta);transformation_matrix (0, 1) = -sin (theta);transformation_matrix (1, 0) = sin (theta);transformation_matrix (1, 1) = std::cos (theta);// A translation on Z axis (0.4 meters)transformation_matrix (2, 3) = 0.4;// Display in terminal the transformation matrixstd::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;print4x4Matrix (transformation_matrix);// Executing the transformationpcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);*cloud_tr = *cloud_icp;// We backup cloud_icp into cloud_tr for later use// 这是ICP对象的创建。 我们设置ICP算法的参数。// setMaximumIterations（iterations）设置要执行的初始迭代次数（默认值为1）。// 然后，我们将点云转换为cloud_icp。 第一次对齐后，我们将在下一次使用该ICP对象时（当用户按下“空格”时）将ICP最大迭代次数设置为1。// The Iterative Closest Point algorithmtime.tic ();pcl::IterativeClosestPoint<PointT, PointT> icp;icp.setMaximumIterations (iterations);icp.setInputSource (cloud_icp);icp.setInputTarget (cloud_in);icp.align (*cloud_icp);icp.setMaximumIterations (1);// We set this variable to 1 for the next time we will call .align () functionstd::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;// 检查ICP算法是否收敛； 否则退出程序。 如果返回true，我们将转换矩阵存储在4x4矩阵中，然后打印刚性矩阵转换。if (icp.hasConverged ()){std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix = icp.getFinalTransformation ().cast<double>();print4x4Matrix (transformation_matrix);}else{PCL_ERROR ("\nICP has not converged.\n");return (-1);}// Visualizationpcl::visualization::PCLVisualizer viewer ("ICP demo");// Create two vertically separated viewportsint v1 (0);int v2 (1);viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);// The color we will be usingfloat bckgr_gray_level = 0.0;// Blackfloat txt_gray_lvl = 1.0 - bckgr_gray_level;// Original point cloud is whitepcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl);viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);// Transformed point cloud is greenpcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);// ICP aligned point cloud is redpcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 180, 20, 20);viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);// Adding text descriptions in each viewportviewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);std::stringstream ss;ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str ();viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);// Set background colorviewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);// Set camera position and orientationviewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);viewer.setSize (1280, 1024);// Visualiser window size// Register keyboard callback :viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);// Display the visualiserwhile (!viewer.wasStopped ()){viewer.spinOnce ();// The user pressed "space" :if (next_iteration){// The Iterative Closest Point algorithmtime.tic ();// 如果用户按下键盘上的任意键，则会调用keyboardEventOccurred函数。 此功能检查键是否为“空格”。// 如果是，则全局布尔值next_iteration设置为true，从而允许查看器循环输入代码的下一部分：调用ICP对象以进行对齐。// 记住，我们已经配置了该对象输入/输出云，并且之前通过setMaximumIterations将最大迭代次数设置为1。icp.align (*cloud_icp);std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;// 和以前一样，我们检查ICP是否收敛，如果不收敛，则退出程序。if (icp.hasConverged ()){// printf（“ 033 [11A”）; 在终端增加11行以覆盖显示的最后一个矩阵是一个小技巧。// 简而言之，它允许替换文本而不是编写新行； 使输出更具可读性。 我们增加迭代次数以更新可视化器中的文本值。printf ("\033[11A");// Go up 11 lines in terminal output.printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());// 这意味着，如果您已经完成了10次迭代，则此函数返回矩阵以将点云从迭代10转换为11。std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;// 函数getFinalTransformation（）返回在迭代过程中完成的刚性矩阵转换（此处为1次迭代）。transformation_matrix *= icp.getFinalTransformation ().cast<double>();// WARNING /!\ This is not accurate! For "educational" purpose only!print4x4Matrix (transformation_matrix);// Print the transformation between original pose and current posess.str ("");ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str ();viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");}else{PCL_ERROR ("\nICP has not converged.\n");return (-1);}//这不是我们想要的。 如果我们将最后一个矩阵与新矩阵相乘，那么结果就是从开始到当前迭代的转换矩阵。}next_iteration = false;}return (0);}

执行如下命令

./interactive_icp../data/result.pcd1

初始

多次迭代之后

如果ICP表现出色，则两个矩阵的值应完全相同，并且ICP找到的矩阵的对角线外的符号应相反。

参考

[1] https://www.sciencedirect.com/book/9780323994484/theories-and-practices-of-self-driving-vehicles
[2] ICP算法https://blog.csdn.net/u014709760/article/details/99241393
[3] https://robot.czxy.com/docs/pcl/chapter03/registration_intro/
[4] 【ICP算法概述及使用SVD推导（组会录像）】 https://www.bilibili.com/video/BV1sL4y147M5/?share_source=copy_web&vd_source=d0320b1c0afda23e433e01c8d114e0a5
[5] 使用 SVD 方法求解 ICP 问题http://www.liuxiao.org/2019/08/%E4%BD%BF%E7%94%A8-svd-%E6%96%B9%E6%B3%95%E6%B1%82%E8%A7%A3-icp-%E9%97%AE%E9%A2%98/