本文说明了一种 ICP 算法，该算法用于深度融合管道（例如 KinectFusion）。

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。

如何最小化目标函数？

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

在高斯-牛顿方法中，我们通过改变R和t来让函数 E 减小的方向（即梯度方向）进行顺序步骤

在每个步骤中，我们通过将其当前值加上雅可比矩阵乘以串联的delta R和delta t向量的delta x来线性地估计函数E。
我们通过求解方程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}$

如何使 <em>E_approx</em> 最小化？

当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 x 1 rshift 向量表示旋转轴乘以旋转角度。我们用罗德里格斯变换来从该变量中获得旋转矩阵。有关更多详情，请参阅维基页面。