ICP算法学习分享3：简单代码实战及算法理解

ICP：Iterative Closest Point，顾名思义，这是一种迭代的思想。通过不断地迭代，每次在前一次的计算结果之上再计算出新的变换矩阵，最终当迭代次数满足条件或者变换矩阵收敛时停止。

设已经过粗配准后待进行细配准的两帧点云分别为P={p1​,p2​,⋯,pm​},Q={q1​,q2​,⋯,qn​}。通常情况下，由于深度相机视野不变，所以扫描出的每一帧点云中点集数量相等，即m = n m=nm=n），这里假设需要将P PP配准到Q QQ上。

 步骤：

1.寻找P和Q上所有对应点对：遍历P或Q中所有点进行操作，如果遍历到Pi点，将Pi点放入Q所有点构成的kdtree空间，查找Q中距离Pi最近的点qi,如果＜，则认为（pi，qi）是一对对应点，其中为人为设定的阈值。（其中值得注意一点，在不同点对（Pi，Qi）与（Pj，Qj）中，完全有可能出现Qi=Qj的情况，这并不影响）遍历完P中的所有点后找到了所有对应点对。

对应点对集合Corr={(pi​,qi​)∣pi​∈P,qi​∈Q,∥pi​−qi​∥<ξ}，其中Qi为Q的kdtree空间与Pi最近的点。

2.设要求的变换矩阵为R，对于∀p∈Corr，都可以构造一个误差lp=∥Rp−q∥，那么对于所有对应点对，就可以利用每个点的误差求和来构造一个整体的误差函数L=∑iN​∥Rpi​−qi​∥，这样我们的目标就成了让这个误差函数最小，就成了一个最小二乘问题。在Eigen库中，只需要输入两片点云中的对应点即可求得变换矩阵R。

3.在第二步中求得变换矩阵R后，将点云P进行变换后得到更新位置后的