ICP（迭代最近点）算法推导详解
概述
ICP用来求解3D-3D的位姿估计问题。假设有一组配对好的3D点，例如对两幅RGB-D图像进行匹配，或者是EPnP方法中已知参考的在相机坐标系中的坐标和在世界坐标系的坐标(参考我PnP的博文)：
P = { p 1 , . . . , p n } , P ′ = { p 1 ′ , . . . , p n ′ } P=\left\{ p_{1},...,p_{n} \right\} , P^{'}=\left\{ p^{'}_{1},...,p^{'}_{n} \right\} P={p1,...,pn},P′={p1′,...,pn′}
现在想要找到R,t使得
∀ i , p i = R p i ′ + t \forall i , p_{i} = Rp^{'}_{i}+t ∀i,pi=Rpi′+t
对应到EPnP问题的时候，就是已知一系列的点在相机坐标系下和世界坐标系下的坐标，求相机坐标系相对于世界坐标系的位姿R，t。
这个问题可以用迭代最近点(Iterative Closest Point, ICP)求解。这里可以看到在3D到3D的映射模型中并没有相机模型，也就是说，仅考虑两组3D点之间的变换时，和相机没有关系。因此在激光SLAM中也会使用ICP，不过由于激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个点，所以这种方法被成为迭代最近点。在视觉中，我们用ICP指代匹配好的两组点间的运动估计问题。
求解方法
- SVD代数方法
- 非线性优化方法（暂时有些问题等搞懂再把博客补上）
SVD代数方法
SVD方法是通过构建最小二乘法进行求解。
首先定义第i对点的误差项：
e i = p i − ( R p i ′ + t ) ( 1 ) e_{i}=p_{i}-(Rp^{'}_{i}+t) \ \ \ (1) ei=pi−(Rpi′+t) (1)
然后构建最小二乘问题，求使误差平方和达到极小的R,t:
m i n R , t 1 2 ∑ i = 1 n ∥ p i − ( R p i ′ + t ) ∥ 2 2 ( 2 ) min_{R,t}\frac{1}{2}\sum_{i=1}^{n}\left\|p_{i}-(Rp_{i}^{'}+t) \right\|_{2}^{2} \ \ (2) minR,t21i=1∑n∥∥∥pi−(Rpi′+t)∥∥∥22 (2)
下面进行求解的推导。
首先，定义两组点的质心：
p = 1 n ∑ i = 1 n ( p i ) p ′ = 1 n ∑ i = 1 n ( p i ′ ) ( 3 ) p=\frac{1}{n}\sum_{i=1}^{n}(p_{i}) \ \ p^{'} =\frac{1}{n}\sum_{i=1}^{n}(p^{'}_{i}) \ \ (3) p=n1i=1∑n(pi) p′=n1i=1∑n(pi′) (3)
注意质心是没有下标的。随后在误差函数中做如下处理：
（4）
上图中的等式应该不用解释，至于为什么这么做我在后面会说！
注意交叉项的部分可以进行化简：
∑ i = 1 n p i − p − R ( p i ′ − p ′ ) = p 1 + p 2 + . . . + p n − n p + R ( p 1 ′ + p 2 ′ + . . . + p n ′ − n p ′ ) \sum_{i=1}^{n}p_{i}-p-R(p_{i}^{'}-p^{'})=p_{1}+p_{2}+...+p_{n}-np+R(p^{'}_{1}+p^{'}_{2}+...+p^{'}_{n}-np^{'}) i=1∑npi−p−R(pi′−p′)=p1+p2+...+pn−np+R(p1′+p2′+...+pn′−np′)
= n p − n p + R ( n p ′ − n p ′ ) = 0 =np-np+R(np^{'}-np^{'})=0 =np−np+R(np′−np′)=0
根据上式交叉项为0，神奇不，真神奇，因此优化的目标函数可以简化为：
m i n R , t J = 1 2 ∑ i = 1 n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 ( 5 ) min_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}\left\| p_{i}-p-R(p^{'}_{i}-p^{'}) \right\|^{2}+\left\|p-Rp^{'}-t \right\|^{2} \ (5) minR,tJ=21i=1∑n∥∥∥pi−p−R(pi′−p′)∥∥∥2+∥∥∥p−Rp′−t∥∥∥2 (5)
我之前没有说(4)式为什么要那么处理，现在我给出我的理解， p i − p p_{i}-p pi−p以及 p i ′ − p ′ p^{'}_{i}-p^{'} pi′−p′是去质心化的过程，这个过程将 p i p_{i} pi与 p i ′ p_{i}^{'} pi′之间距离的衡量转化为每个点与各自质心的相对位置关系，其实相当于抹取了平移这一尺度，所以最终我们观察公式(5)可以发现左边只和旋转矩阵R相关，而右边既有R，又有t，但只和质心有关。只要我们获得了R，令第二项为零就能得到t。
于是，ICP可以分为以下三个步骤求解
-
- 计算两组点的质心位置 p p p, p ′ p^{'} p′,然后计算每个点的去质心坐标：
q i = p i − p q i ′ = p i ′ − p ′ q_{i}=p_{i}-p \ \ \ q_{i}^{'}=p_{i}^{'}-p^{'} qi=pi−p qi′=pi′−p′
- 计算两组点的质心位置 p p p, p ′ p^{'} p′,然后计算每个点的去质心坐标：
- 2.根据以下优化问题计算旋转矩阵， R ∗ R^{*} R∗为最优化的旋转矩阵：
R ∗ = a r g m i n R 1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2 R^{*}=arg min_{R}\frac{1}{2}\sum_{i=1}^{n}\left\| q_{i} -Rq_{i}^{'} \right\|^{2} R∗=argminR21i=1∑n∥∥∥qi−Rqi′∥∥∥2 -
- 根据第二步的 R R R计算 t t t， t ∗ t^{*} t∗为最优化的平移向量：
t ∗ = p − R p ′ t^{*}= p-Rp^{'} t∗=p−Rp′
- 根据第二步的 R R R计算 t t t， t ∗ t^{*} t∗为最优化的平移向量：
我们看到，只要求出了两组点之间的旋转，平移量是非常容易得到的。所以我们重点关注R的计算。展开R的误差项，得
1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2 = 1 2 ∑ i = 1 n ( q i T q i + ( q i ′ ) T R T R q i ′ − 2 q i T R q i ′ ) ( 6 ) \frac{1}{2}\sum_{i=1}^{n}\left\| q_{i} -Rq_{i}^{'} \right\|^{2}=\frac{1}{2}\sum_{i=1}^{n}(q^{T}_{i}q_{i}+(q_{i}^{'})^{T}R^{T}Rq_{i}^{'}-2q_{i}^{T}Rq_{i}^{'}) \ \ (6) 21i=1∑n∥∥∥qi−Rqi′∥∥∥2=21i=1∑n(qiTqi+(qi′)TRTRqi′−2qiTRqi′) (6)
注意到第一项与R无关，第二项由于R^{T}R=I，亦与R无关。因此，实际上优化目标函数变为：
（7）
（7式中第一个等号，是将矩阵的二次型用迹表示，数学公式为 x T A x = t r ( A x x T ) x^{T}Ax=tr(Axx^{T}) xTAx=tr(AxxT)。
为了解R，先定义矩阵：
W = ∑ i = 1 n q i ( q i ′ ) T W=\sum_{i=1}^{n}q_{i}(q_{i}^{'})^{T} W=i=1∑nqi(qi′)T
W是一个3x3的矩阵，对W进行SVD分解，得
W = U ∑ V T W=U\sum V^{T} W=U∑VT
解得 R = ± U V T R=\pm UV^{T} R=±UVT
上面这个SVD解很多人肯定有疑问，这个我在PnP算法详解（超详细公式推导)DLT手写推导处进行了推导，道理都是一样的，如果你不懂一定去看我的PnP文章，况且我感觉你学了ICP也得学PnP。
这里是两个解，选择R的行列式为正的解。解得R后求t即可。
- 高翔老师讲的ICP到这里结束了，但这里说一下，我们在写代码或者看一些库函数实现的时候一般要加上迭代的过程即使你是用的SVD代数方法。
- 迭代很简单就是通过你解出来的R，t，代入到(4)中得到新的 p i ′ p_{i}^{'} pi′，然后根据新的点坐标再进行优化，反复进行直到你的误差达小于你设定的阈值就可以将最后的R，t最为结果了。如果你学过机器学习或者最优化理论这非常好理解。
非线性优化法
这个方法我还有些问题，等我弄明白再补上去
参考：
视觉SLAM十四讲第二版