ICP 点到平面测距算法 OpenCV v4.8.0

本文介绍了用于深度融合管道（如 KinectFusion）的 ICP 算法。

ICP 的目标是对齐两个点云，一个是旧点云（三维模型中的现有点和法线），另一个是新点云（新点和法线，即我们要整合到现有模型中的点和法线）。ICP 返回这两个点云之间的旋转和平移变换。

迭代最近点（ICP）最小化目标函数，即两个点云中相应点之间的点到平面距离（PPD）：

$$E=\sum {}_i||ppd\left(p_i,q_i,n_i\right)|{|}_2\to 0$$

什么是 ppd（p, q, n）？

具体来说，对于每个对应点 P 和 Q，它是点 P 到由点 Q 和位于点 Q 上的法线 N 所决定的平面的距离。

p - 新点云中的第 i 个点

q - 旧点云中的第 i 个点

n - 旧点云中 q 点的法线

因此， ppd（…） 可以表示为（p 和 q 之间的差值）和（n）的点积：
$$dot\left(T_{p2q}\left(p\right)-q,n\right)=dot\left(\left(R\cdot p+t\right)-q,n\right)={\left[\left(R\cdot p+t\right)-q\right]}^T\cdot n$$
T§ 是点 p 的刚性变换：
$$T_{p2q}\left(p\right)=\left(R\cdot p+t\right)$$
T 是我们通过 ICP 搜索的变换，其目的是使每个点 p 与对应点 q 的点到平面距离更近。

如何最小化目标函数？

我们使用高斯-牛顿法进行函数最小化。

在高斯-牛顿法中，我们沿着函数 E 的下降方向，即梯度方向，通过改变 R 和 t 来完成连续步骤：

1. 在每一步中，我们将函数 E 线性近似为其当前值加上雅各布矩阵乘以 delta x，后者是由 delta R 和 delta t 向量串联而成。
2. 我们通过求解方程 E_approx(delta_x) = 0 来找到 delta R 和 delta t。
3. 我们将 delta R 和 delta t 应用于当前的 Rt 变换，然后进行下一次迭代

如何对 E 进行线性化？

让我们在无限小邻域内对其进行近似。

下面是我们要通过改变 R 和 t 使其最小化的公式：
$$E=\sum ||{\left[\left(R\cdot p+t\right)-q\right]}^T\cdot n|{|}_2$$
虽然点到平面的距离与 R 和 t 都是线性关系，但旋转空间本身并不是线性的。从 R 是如何从旋转角度生成的就可以看出这一点：
$$R=R_z\left(\gamma \right)R_y\left(\beta \right)R_x\left(\alpha \right)=\left[\begin{array}{ccc}\cos \left(\gamma \right)& -\sin \left(\gamma \right)& 0\\ \sin \left(\gamma \right)& \cos \left(\gamma \right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \left(\beta \right)& 0& \sin \left(\beta \right)\\ 0& 1& 0\\ -\sin \left(\beta \right)& 0& \cos \left(\beta \right)\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\alpha \right)& -\sin \left(\alpha \right)\\ 0& \sin \left(\alpha \right)& \cos \left(\alpha \right)\end{array}\right]$$
但由于我们有无限小的旋转，R 可以用下面的形式近似表示：
$$R=I+Ad\theta$$
其中 I - 单位矩阵，A - 三维特殊正交群 so(3) 的成员。

将所有 sin(t) 和 cos(t) 项逼近其 t -> 0 时的极限，可以得到以下表示：
$$R=I+\left[\begin{array}{ccc}0& -\gamma & \beta \\ \gamma & 0& -\alpha \\ -\beta & \alpha & 0\end{array}\right]=I+skew\left({\left[\begin{array}{ccc}\alpha & \beta & \gamma \end{array}\right]}^T\right)=I+skew\left(R_{shift}\right)$$
将 R 的近似值代入 E 的表达式，就得到了：
$$E_{approx}=\sum ||{\left[\left(I+skew\left(R_{shift}\right)\right)\cdot p+t-q\right]}^T\cdot n|{|}_2$$
$$E_{approx}=\sum ||{\left[I\cdot p+skew\left(R_{shift}\right)\cdot p+t-q\right]}^T\cdot n|{|}_2$$

$$E_{approx}=\sum ||{\left[skew\left(R_{shift}\right)\cdot p+t+p-q\right]}^T\cdot n|{|}_2$$
让我们引入一个近似变换移动的函数 f：
$$f\left(x,p\right)=skew\left(R_{shift}\right)\cdot p+t$$
$$E_{approx}=\sum ||{\left[f\left(x,p\right)+p-q\right]}^T\cdot n|{|}_2$$

如何最小化 E_approx？

当 E_approx 的差值（即参数增加的导数）为零时，E_approx 就是最小的：
$$d\left(E_{approx}\right)=\sum {}_{i}d\left(||ppd\left(T_{approx}\left(p_i\right),q_i,n_i\right)|{|}_2\right)$$
$$\sum {}_{i}d\left(ppd{\left(T_{approx}\left(p_i\right),q_i,n_i\right)}^2\right)=\sum {}_{i}2\cdot ppd\left(...\right)\cdot d\left(ppd\left(T_{approx}\left(p_i\right),q_i,n_i\right)\right)$$
让我们微分一下 ppd：
$$d\left(ppd\left(T_{approx}\left(p_i\right),q_i,n_i\right)\right)=d\left({\left[f\left(x,p_i\right)+p_i-q_i\right]}^T\cdot n_i\right)=df{\left(x,p_i\right)}^T\cdot n_i=d{x}^T{f}^{\prime }{\left(x,p_i\right)}^T\cdot n_i$$
下面是我们从向量 x 中得到的所有变量 x_j 的结果：
$$\frac{\partial E}{\partial x_j}=\sum \left[2\cdot {\left(f\left(x,p\right)+p-q\right)}^T\cdot n\right]\cdot \left[f_j^{\prime }{\left(x,p\right)}^T\cdot n\right]=0$$
$$\sum \left[2\cdot n^T\cdot {\left(f\left(x,p\right)+p-q\right)}^T\right]\cdot \left[n^T\cdot f^{\prime }\left(x,p\right)\right]=0$$

让新变量：
$$\Delta p=p-q$$
$$\sum \left[2\cdot n^T\cdot {\left(f\left(x,p\right)+\Delta p\right)}^T\right]\cdot \left[n^T\cdot f^{\prime }\left(x,p\right)\right]=0$$
$$\sum \left[{\left(f\left(x,p\right)+\Delta p\right)}^T\cdot \left(n\cdot n^T\right)\right]\cdot f^{\prime }\left(x,p\right)=0$$
$$\sum f^{\prime }\left(x,p\right)\cdot \left[n\cdot n^T\right]\cdot {\left(f\left(x,p\right)+\Delta p\right)}^T=0$$
f(x, p) 可以表示为矩阵与向量相乘。要证明这一点，我们必须记住
$$cross\left(a,b\right)=skew\left(a\right)\cdot b=skew{\left(b\right)}^T\cdot a$$
$$f\left(x,p\right)=skew\left(R_{shift}\right)\cdot p+t_{shift}=skew{\left(p\right)}^T{R}_{shift}+t_{shift}$$
$$f\left(x,p\right)=\left[skew{\left(p\right)}^T{I}_{3\times 3}\right]\cdot {\left[\Delta R\Delta t\right]}^T=G\left(p\right)\cdot x$$
引入 G§ 是为了简化计算。

因为$$df\left(x,p\right)=G\left(p\right)\cdot dx=f^{\prime }\left(x,p\right)\cdot dx$$
我们得到：
$$f^{\prime }\left(x,p\right)=G\left(p\right)$$
$$\sum f^{\prime }{\left(x,p\right)}^T\cdot \left[n\cdot n^T\right]\cdot \left[f\left(x,p\right)\right]=\sum f^{\prime }{\left(x,p\right)}^T\cdot \left[n\cdot n^T\right]\cdot \left[-\Delta p\right]$$
$$\sum G{\left(p\right)}^T\cdot \left[n\cdot n^T\right]\cdot \left[G\left(p\right)\cdot X\right]=\sum G{\left(p\right)}^T\cdot \left[n\cdot n^T\right]\cdot \left[-\Delta p\right]$$
让一个新值：
$$C=G{\left(p\right)}^T\cdot n$$
$$C^T={\left(G{\left(p\right)}^T\cdot n\right)}^T=n^T\cdot G\left(p\right)$$
让我们来替换一下：
$$\sum C\cdot C^T\cdot X=\sum C\cdot n^T\cdot \left[-\Delta p\right]$$
$$\sum C\cdot C^T\cdot \left[\begin{array}{c}\Delta R\\ \Delta t\end{array}\right]=\sum C\cdot n^T\cdot \left[-\Delta p\right]$$
通过求解这个方程，我们可以得到每次高斯-牛顿迭代的刚性变换位移。

如何应用变换位移？

我们从变换中生成旋转和平移矩阵，然后将当前姿态矩阵乘以我们得到的矩阵。

移位的平移部分会原封不动地加入到生成的矩阵中，而旋转部分的生成则比较麻烦。旋转偏移通过指数化从 so(3) 转换为 SO(3)。事实上，3 乘 1 的 rshift 向量代表旋转轴乘以旋转角度。我们使用罗德里格斯变换从中得到旋转矩阵。更多详情，请参阅维基页面。