本文描述了深度融合管道（例如 KinectFusion）中使用的 ICP 算法。

ICP 的目标是对齐两个点云，旧点云（3D 模型中已存在的点和法线）和新点云（新的点和法线，我们想要集成到现有模型中的）。 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_ 和位于点 _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_。

如何最小化目标函数？

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

在高斯-牛顿法中，我们通过在函数 E 减小的方向上，即在其梯度的方向上改变 _R_ 和 _t_ 来执行顺序步骤

在每个步骤中，我们将函数 _E_ 线性近似为其当前值加上雅可比矩阵乘以 _delta x_，其中 _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_ 的微分（即，按参数增加的导数）为零时，它是最小的，因此让我们找到该微分

$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)_。事实上，3-by-1 rshift 向量表示旋转轴乘以旋转角度。我们使用 Rodrigues 变换从该向量中获得旋转矩阵。更多详情，请参见维基页面。