ICP——迭代最近点

Posted on Edited on In

ICP（iterative closest point）最广泛的应用应该就是点云的配准了。它的提出也是为了解决这个问题。

假设我们有两个点云，它们应该是一一配对的（或者说在SLAM中两个场景有较大的overlap），但是我们并不知道点的配对关系。现在我们希望根据这两个点云的位置求到相机的位姿变化，也就是旋转矩阵RR与平移tt。

假设这两个点云分别为p,p’p,p’，那么我们知道$p = Rp’ + t 。想要求得。想要求得R,t$，首先要做的是配对关系。而ICP非常简单，它选取距离最近（一般来说为欧几里得距离）的点作为配对的点。因此，现在我们有了两组配对点： P={p1,p2,...,pn},P′={p1′,p2′,...,pn′} P = \{p_1,p_2,...,p_n\}, P' = \{p'_1,p'_2,...,p'_n\} 我们希望做的是最小化下面这个代价： J=∑i=1n‖pi′−(Rpi+t)‖2 J = \sum_ {i=1}^n\Vert p'_i - (Rp_i+t)\Vert^2 如何解这其中的RR和tt？比较容易想到的就是最小二乘法，下面我们推导介绍一下利用SVD来解决这个最小二乘问题。

首先定义点云的质心为： p=1n∑i=1npi,p′=1n∑i=1npi′ p = \frac 1 n \sum_ {i=1}^np_i, p' = \frac{1}{n} \sum_ {i=1}^np'_i 则: ∑i=1n‖pi′−(Rpi+t)‖2=∑i=1n‖pi′−Rpi−t−p′+Rp+p′−Rp‖2=∑i=1n‖(pi′−p′−R(pi−p))+(p′−Rp−t)‖2=∑i=1n(‖pi′−p′−R(pi−p)‖2+‖p′−Rp−t‖2+2(pi′−p′−R(pi−p))T(p′−Rp−t)) \begin{aligned} \sum_ {i=1}^n\Vert p'_i - (Rp_i + t) \Vert^2 &= \sum_ {i=1}^n\Vert p'_i- Rp_i - t - p' + Rp + p' - Rp \Vert^2\\ &=\sum_ {i=1}^n\Vert (p'_i-p'-R(p_i-p)) + (p'-Rp - t) \Vert^2\\ &= \sum_ {i=1}^n(\Vert p'_i-p'-R(p_i-p)\Vert^2 + \Vert p'-Rp - t \Vert^2 + 2(p'_i-p'-R(p_i-p))^T(p'-Rp - t)) \end{aligned} 值得注意的是：p’i−p’−R(pi−p)p’_i-p’-R(p_i-p)在求和之后为0，这是由质心的定义决定的。

因此原来的问题就简化为： minR,tJ=∑i=1n(‖pi′−p′−R(pi−p)‖2+‖p′−Rp−t‖2 \min_ {R,t} J = \sum_ {i=1}^n(\Vert p'_i-p'-R(p_i-p)\Vert^2 + \Vert p'-Rp - t \Vert^2 观察之后，我们发现第一个式子只和旋转有关，第二个式子和旋转平移都有关，不过另一方面它只和质心相关。如果我们求得了RR，简单的令第二个式子为0就可以求得对应的tt。所以现在ICP的算法表述如下：

令qi,q’iq_i,q’_i为原来点的去质心坐标： qi=pi−p,qi′=pi′−p′ q_i = p_i - p,q'_i = p'_i - p' 计算最佳的RR: R*=arg⁡minR∑i=1n‖qi′−Rqi‖2 R^* = \arg\min_R \sum_ {i=1}^n\Vert q'_i - Rq_i\Vert^2 最后，根据R*R^*计算tt: t*=p′−R*p t^* = p' - R^{*}p 因此求得RR之后，tt是非常容易得到的。 ∑i=1n‖qi′−Rqi‖2=∑i=1n(q′iTqi′+qiTRTRqi−2q′iTRqi) \sum_ {i=1}^n\Vert q'_i-Rq_i\Vert^2 = \sum_ {i=1}^n( {q'}_i^Tq'_i + q_i^TR^TRq_i - 2{q'}_i^TRq_i) 第一项与RR无关，而RTR=IR^TR=I，因此第二项也无所谓。我们重点要做的是min⁡R∑i=1n−q’iTRqi\min_R \sum_ {i=1}^n -{q’}_i^TRq_i。 ∑i=1n−q′iTRqi=∑i=1n−tr(Rqiq′iT)=−tr(R∑i=1nqiq′iT) \sum_ {i=1}^n -{q'}_i^TRq_i = \sum_ {i=1}^n -\text{tr}(Rq_i{q'}_ i^T) = -\text{tr}\left(R\sum_ {i=1}^nq_i{q'}_i^T\right) 计算这个RR可以使用SVD来解决。

定义W=∑i=1nq’iqiTW = \sum_ {i=1}^n q’_iq_i^T，根据SVD得到：

W = UV^T

则R*=UVTR^* = UV^T。如果rank(W)=3\text{rank}(W)=3，则RR是唯一最优解。具体的证明比较复杂。这就求得了从pp到p’p’的旋转，有了旋转矩阵，平移就非常容易得到了。

至于迭代的过程，就是我们根据求得的R,tR,t进行转换后，重复上述的步骤。

最早的关于ICP的论文为A Method for Registration of 3-D Shapes。