ICP理解

ICP是什么？

第一次遇见ICP是在高博的《视觉SLAM十四讲》上，上面很简要的说了一下要求解两组3D点集P={p1,…pn}和P’={p’1,…p’n}之间的欧式变换R,t,使得对于任意的pi=Rp’i+t,可以使用ICP，也叫迭代最近点，ICP的求解方法介绍了SVD方法。ICP算法本质是基于最小二乘的最优配准方法。该方法重复进行选择对应关系对，计算最优刚体变换，直到满足正确配准的收敛精度要求。

ICP目标

ICP算法的目的就是找到目标点集target cloud(不动）P与参考点集source cloud（要变化）Q之间最优的旋转参数R和平移参数T,使得（Pi-R*Qi-T）的二范数的和最小。

ICP算法步骤

1. 寻找对应点可是，我们现在并不知道有哪些对应点。因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对source cloud进行变换（或者是找source cloud在target cloud中的最近点，欧式距离作为判据），得到的一个变换后的点云。然后将这个变换后的点云与target cloud进行比较，只要两个点云中存在距离小于一定阈值，我们就认为这两个点就是对应点。
2. R、T优化有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。采用最小二乘等方法求解最优的旋转平移矩阵。去质心，SVD分解,参考《视觉SLAM十四讲》。

迭代我们优化得到了一个新的R与T，将R和T作用于source cloud得到新的变换点集，如果新的变换点集与目标点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则将新的变换点集作为新的source cloud继续迭代，直到达到目标函数的要求。

参考原文：https://blog.csdn.net/fyf18845165207/article/details/83246800