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

ICP的目标是配准两组点云：旧点云（三维模型中已存在的点和法向量）和新点云（新的点和法向量，我们需要将其整合到现有模型中）。ICP返回这两组点云之间的旋转和平移变换。

迭代最近点（ICP）算法最小化目标函数，该函数表示两组点云中对应点之间的点到平面距离（PPD）。

$E=\sum_{i}\left\|ppd(p_{i}, q_{i}, n_{i})\right\|_{2}\rightarrow0$

什么是ppd(p, q, n)？

具体来说，对于每对对应点P和Q，它是点P到由点Q及其法向量N确定的平面的距离。如果在当前相机姿态下，两点P和Q投影到同一个像素，则认为它们是对应的。

p - 新点云中的第i个点

q - 旧点云中的第i个点

n - 旧点云中点q处的法向量

因此，ppd(...)可以表示为（p和q的差）与（n）的点积。

$dot(T_{p2q}(p)-q, n)=dot((R\cdot p + t)-q,n)=[(R\cdot p + t)-q]^{T}\cdot n$

T(p)是点p的刚体变换

$T_{p2q}(p) = (R \cdot p + t)$

其中R - 旋转矩阵，t - 平移向量。

T是通过ICP搜索得到的变换，其目的是在点到平面距离方面使每个点p更接近其对应的点q。

如何最小化目标函数？

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

在高斯-牛顿法中，我们通过改变R和t来进行顺序迭代，方向为函数E减小的方向，即其梯度的方向。

在每一步中，我们将函数E线性近似为其当前值加上雅可比矩阵乘以delta x（delta R和delta t向量的连接）。
我们通过求解方程E_approx(delta_x) = 0来找到delta R和delta t。
我们将delta R和delta t应用于当前的Rt变换，并进行下一次迭代。

如何线性化E？

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

这是一个我们将通过改变R和t来最小化的公式。

$E=\sum\left\|[(R\cdot p + t)-q]^{T}\cdot n\right\|_{2}$

虽然点到平面距离对R和t都是线性的，但旋转空间本身并不是线性的。你可以从R如何由其旋转角生成中看出这一点。

$\displaystyle R = R_{z}(\gamma)R_{y}(\beta )R_{x}(\alpha)= \begin{bmatrix} cos(\gamma) & -sin(\gamma) & 0 \\ sin(\gamma) & cos(\gamma) & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} cos(\beta) & 0 & sin(\beta)\\ 0 & 1 & 0\\ -sin(\beta) & 0 & cos(\beta) \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & cos(\alpha) & -sin(\alpha)\\ 0 & sin(\alpha) & cos(\alpha) \end{bmatrix}$

但是由于我们有无穷小的旋转，R可以近似为以下形式：

$R=I + Ad\theta$

其中I - 单位矩阵，A - 三维特殊正交群so(3)的成员。

通过将所有sin(t)和cos(t)项逼近到其极限（t --> 0），我们得到以下表示：

$R = I + \begin{bmatrix}0 & -\gamma & \beta \\ \gamma & 0 & -\alpha \\ -\beta & \alpha & 0 \end{bmatrix} = I + skew(\begin{bmatrix} \alpha & \beta & \gamma \end{bmatrix}^{T}) = I + skew(R_{shift})$

将R的近似值代回E表达式，我们得到：

$E_{approx}=\sum\left\|[(I + skew(R_{shift})) \cdot p + t - q]^{T} \cdot n \right \|_{2}$

$E_{approx} = \sum \left \| [I \cdot p + skew(R_{shift}) \cdot p + t - q]^{T} \cdot n \right \|_{2}$

$E_{approx} = \sum \left \| [skew(R_{shift}) \cdot p + t + p- q]^{T} \cdot n \right \|_{2}$

让我们引入一个函数f，它近似变换位移：

$f(x, p) = skew(R_{shift}) \cdot p + t$

$E_{approx} = \sum \left \| [f(x, p) + p- q]^{T} \cdot n \right \|_{2}$

如何最小化E_approx？

当E_approx的微分（即自变量增加时的导数）为零时，E_approx最小，所以让我们找到这个微分。

$d(E_{approx}) = \sum_i d(\left \| ppd(T_{approx}(p_i), q_i, n_i) \right \|_2) =$ $\sum_i d(ppd(T_{approx}(p_i), q_i, n_i)^2) =$ $\sum_i 2\cdot ppd(...)\cdot d(ppd(T_{approx}(p_i), q_i, n_i))$

让我们对ppd进行微分：

$d(ppd(T_{approx}(p_i), q_i, n_i)) = d([f(x, p_i) + p_i- q_i]^{T} \cdot n_i) = df(x, p_i)^{T} \cdot n_i = dx^T f'(x, p_i)^T \cdot n_i$

对于向量x中的所有变量x_j，我们得到：

$\frac{\partial E}{\partial x_{j}} = \sum [2 \cdot (f(x, p) + p - q)^{T} \cdot n] \cdot [f_{j}'(x, p)^{T} \cdot n] = 0$

$\sum [2 \cdot n^{T} \cdot (f(x, p) + p - q)] \cdot [n^{T} \cdot f{}'(x, p)] = 0$

令新变量： $\triangle p = p - q$

$\sum [2 \cdot n^{T} \cdot (f(x, p) + \triangle p)] \cdot [n^{T} \cdot f{}'(x, p)] = 0$

$\sum [(f(x, p) + \triangle p)^{T} \cdot (n \cdot n^{T})] \cdot f{}'(x, p) = 0$

$\sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [f(x, p) + \triangle p] = 0$

f(x, p)可以表示为矩阵向量乘法。为了证明这一点，我们必须记住 $cross(a, b) = skew(a) \cdot b = skew(b)^{T} \cdot a$

$f(x, p) = skew(R_{shift}) \cdot p + t_{shift} = skew(p)^T R_{shift} + t_{shift}$ $f(x, p) = \begin{bmatrix} skew(p)^{T} & I_{3\times 3}\end{bmatrix} \cdot \begin{bmatrix} \triangle R & \triangle t \end{bmatrix}^{T} = G(p) \cdot x$

为了简化，引入G(p)。

由于 $d(f(x, p)) = G(p) \cdot dx = f'(x, p) \cdot dx$ ，我们得到 $f'(x, p) = G(p)$ 。

$\sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [f(x, p)] = \sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [- \triangle p]$

$\sum G(p)^{T} \cdot [n \cdot n^{T}] \cdot [G(p) \cdot X] = \sum G(p)^{T} \cdot [n \cdot n^{T}] \cdot [- \triangle p]$

令一个新的值：

$C = G(p)^{T} \cdot n$

$C^{T} = (G(p)^{T} \cdot n)^{T} = n^{T} \cdot G(p)$

让我们进行替换：

$\sum C \cdot C^{T} \cdot X = \sum C \cdot n^{T} \cdot [- \triangle p]$

$\sum C\cdot C^{T}\cdot \begin{bmatrix} \triangle R\\ \triangle t \end{bmatrix} = \sum C \cdot n^{T} \cdot [- \triangle p]$

通过求解这个方程，我们得到每次高斯-牛顿迭代的刚体变换位移。

我们如何应用变换位移？

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

虽然位移的平移部分按原样贡献于结果矩阵，但旋转部分的生成则较为复杂。旋转位移通过指数运算从so(3)转换为SO(3)。事实上，3x1的rshift向量表示旋转轴乘以旋转角度。我们使用罗德里格斯变换从该向量得到旋转矩阵。更多细节，请参见维基页面。