ICP算法的全称是迭代最近邻点（iterative closest points），是用于求解空间点云（或者surface）刚性变化的一种算法。ICP每一个迭代过程中，对位姿的求解都是一个非线性最小二乘问题。本文讲解如何将ICP的非线性最小二乘近似转化为一个线性最小二乘问题并求解。
经典ICP算法在每次迭代过程中首先在源（source）surface和目标（destination）surface之间建立点的对应，通常source surface某点的对应点就是destination surface上距离其最近的点，根据最小化对应点之间的误差，得到source surface和destination surface之间的变换矩阵。
对应点之间的误差有很多度量方式，有point-to-point，point-to-plane，plane-to-plane，这里我们重点讲point-to-plane度量的ICP。最小化point-to-plane误差就是最小化source surface上的点和destination surface上对应点切平面距离的平方和，如图1所示。
图1.point-to-plane误差计算示意
source surface和destination surface之间的变换可由图2所示公式求得。
图2.point-to-plane误差度量。s表示source surface上的点，d表示destination source上对应的点，n表示destination source上对应点的法向量，M表示source surface和destination surface之间的刚性变换。
如果用矩阵的形式表示变换M，那么就有如图3所示的形式。
图3.变换矩阵M由旋转部分R和平移部分T两部分组成。其中α，β和γ分别为绕x轴，y轴和z轴旋转的角度。
由上可见，变换矩阵中的旋转部分是非线性的，导致ICP求解实质上是一个非线性最小二乘问题。我们考虑到，如果在短时间内的运动非常小，这样α，β和γ都接近于0，我们近似地认为sinα=α，sinβ=β，sinγ=γ，cosα，cosβ和cosγ都等于1，如图4所示，这样我们就可以将其近似地转换为一个线性最小二乘问题。
图4.将旋转矩阵近似处理后的结果
由此近似，我们要求解的目标函数就可以写成如下图5的形式。
图5.取近似后目标函数的展开形式
上文所述我们预先建立了source surface和destination surface之间的对应点，每一组对应点都要求满足图5给出的形式，我们以矩阵的方式表示所有对应点之间的关系，如图6所示。
图6.将对应点带入并用矩阵形式表示的目标函数
由此一来，我们对图2中目标函数的求解就变成了如下图7形式，只需要求解用SVD分解的方法求解x即可，如图8所示。
图7.最终要求解的目标函数
图8.SVD分解求解x
以上就是算法的全部介绍。
持续更新，欢迎提出质疑或与作者就相关问题进行讨论。
*参考文献
Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration
