视觉SLAM笔记（38） 3D-3D: ICP

---

1. 3D-3D 的位姿估计问题

假设有一组配对好的 3D 点
比如对两个 RGB-D 图像进行了匹配：P = {p₁, …, p_n}，P’ = {p’₁, …, p’_n}
现在，想要找一个欧氏变换 R, t，使得：

这个问题可以用 迭代最近点（Iterative Closest Point, ICP）求解

在 3D-3D 位姿估计问题中，并没有出现相机模型
也就是说，仅考虑两组 3D 点之间的变换时，和相机并没有关系

因此，在激光 SLAM 中也会碰到 ICP
不过由于激光数据特征不够丰富，无从知道两个点集之间的匹配关系
只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点
而在视觉中，特征点提供了较好的匹配关系，所以整个问题就变得更简单了

在 RGB-D SLAM 中，可以用这种方式估计相机位姿
和 PnP 类似， ICP 的求解也分为两种方式：
利用线性代数的求解（主要是 SVD），以及利用非线性优化方式的求解（类似于 Bundle Adjustment）

---

2. SVD 方法

以 SVD 为代表的线性代数方法
根据前面描述的 ICP 问题，先定义第 $i$ 对点的误差项：

然后，构建最小二乘问题，求使误差平方和达到极小的 R, t：

下面来推导它的求解方法
首先，定义两组点的质心：

请注意质心是没有下标的
随后，在误差函数中，作如下的处理：

注意到交叉项部分中， $(p$_i $− p − R (p$_i $′ − p′))$ 在求和之后是为零的

因此优化目标函数可以简化为：

仔细观察左右两项，发现左边只和旋转矩阵 R 相关，而右边既有 R 也有 t，但只和质心相关
只要获得了 R，令第二项为零就能得到 t

于是， ICP 可以分为以下三个步骤求解：

看到，只要求出了两组点之间的旋转，平移量是非常容易得到的
所以重点关注 R 的计算，展开关于 R 的误差项，得：

注意到第一项和 $R$ 无关，第二项由于 $R$^T $R = I$，亦与 $R$ 无关
因此，实际上优化目标函数变为：

接下来，就是怎样通过 SVD 解出上述问题中最优的 R
但是关于最优性的证明较为复杂，就不详细叙述

为了解 R，先定义矩阵：

$W$ 是一个 3 × 3 的矩阵，对 $W$ 进行 SVD 分解，得：

其中， Σ 为奇异值组成的对角矩阵，对角线元素从大到小排列，而 U 和 V 为正交矩阵

当 W 满秩时， R 为：

解得 R 后，按下式求解 t 即可：

---

3. 非线性优化方法

求解 ICP 的另一种方式是使用非线性优化，以迭代的方式去找最优值
该方法和 视觉SLAM笔记（36） 3D-2D: PnP 非常相似
以李代数表达位姿时，目标函数可以写成：

单个误差项关于位姿导数已经在前面推导过了，使用李代数扰动模型即可：

于是，在非线性优化中只需不断迭代，就能找到极小值
而且， ICP问题存在唯一解或无穷多解的情况
在唯一解的情况下，只要能找到极小值解
那么这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况
这也意味着 ICP 求解可以任意选定初始值
这是已经匹配点时求解 ICP 的一大好处

需要说明的是，这里讲的 ICP，是指已经由图像特征给定了匹配的情况下，进行位姿估计的问题
在匹配已知的情况下，这个最小二乘问题实际上具有解析解，所以并没有必要进行迭代优化
ICP 的研究者们往往更加关心匹配未知的情况
不过，在 RGB-D SLAM 中，由于一个像素的深度数据可能测量不到
所以我们可以混合着使用 PnP 和 ICP 优化：对于深度已知的特征点，用建模它们的 3D-3D 误差
对于深度未知的特征点，则建模 3D-2D 的重投影误差
于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（37） 求解 PnP
视觉SLAM笔记（36） 3D-2D: PnP
视觉SLAM笔记（35） 三角化求特征点的空间位置
视觉SLAM笔记（34） 三角测量
视觉SLAM笔记（33） 对极约束求解相机运动

---