《视觉SLAM十四讲》学习笔记-3D-3D位姿估计-ICP

ICP问题

问题描述：假设匹配好的3D点为：

P = {\vec{p}}_{1}, \dots, {\vec{p}}_{n}, P^{'} = {\vec{p}}_{1}^{'}, \dots, {\vec{p}}_{n}^{'}

$\mathbf{P} = { \vec{p}_1, \cdots, \vec{p}_n }, ~~\mathbf{P}' = { \vec{p}'_1, \cdots, \vec{p}'_n }$
想要找一个变换

R, \vec{t}

$\mathbf{R}, \vec{t}$ ， s.t.:

\forall i, {\vec{p}}_{i} = R {\vec{p}}_{i}^{'} + \vec{t}

$\forall i, \vec{p}_i = \mathbf{R}\vec{p}'_i + \vec{t}$
此问题可用 迭代最近点来求解(Iterative Closest Point, ICP). 为描述方便， ICP统指匹配好的两组点间运动估计问题。

ICP的求解主要有线性代数的求解(SVD)和非线性优化的求解(BA).

线性的方法：SVD

首先定义第 $i$ 对点的误差项为:

{\vec{e}}_{i} = {\vec{p}}_{i} - (R)

$\vec{e}_i = \vec{p}_i - (\mathbf{R})$
首先定义第

i

$i$ 对的误差项：

{\vec{e}}_{i} = {\vec{p}}_{i} - R {\vec{p}}_{i}^{'} + \vec{t}

$\vec{e}_i = \vec{p}_i - \mathbf{R}\vec{p}'_i+\vec{t}$
则其最小二乘问题为：

min_{R, \vec{t}} J = \frac{1}{2} \sum_{i = 1}^{n} ‖ {\vec{p}}_{i} - (R {\vec{p}}_{i}^{'} + \vec{t}) ‖_{2}^{2}

$\underset{\mathbf{R},\vec{t}} {\min} ~J=\frac{1}{2}\sum_{i=1}^{n} \|\vec{p}_i-(\mathbf{R}\vec{p}'_i + \vec{t})\|_2^2$
为解上式，定义两组点的质心为：

\vec{p} = \frac{1}{n} \sum_{i = 1}^{n} {\vec{p}}_{i}, {\vec{p}}^{'} = \frac{1}{n} \sum_{i = 1}^{n} {\vec{p}}_{i}^{'}

$\vec{p} = \frac{1}{n}\sum_{i=1}^{n}\vec{p}_i, ~~ \vec{p}' = \frac{1}{n}\sum_{i=1}^{n}\vec{p}'_i$
展开误差函数：

\begin{aligned} \frac{1}{2} \sum_{i = 1}^{n} ‖ {\vec{p}}_{i} - (R {\vec{p}}_{i}^{'} + \vec{t}) ‖_{2}^{2} & = \frac{1}{2} \sum_{i = 1}^{n} ‖ {\vec{p}}_{i} - R {\vec{p}}_{i}^{'} - \vec{t} - \vec{p} + R {\vec{p}}^{'} + \vec{p} - R {\vec{p}}^{'} ‖_{2}^{2} \\ = \frac{1}{2} \sum_{i = 1}^{n} ‖ ({\vec{p}}_{i} - \vec{p} - R ({\vec{p}}_{i}^{'} - {\vec{p}}^{'})) + (\vec{p} - R {\vec{p}}^{'} - \vec{t}) ‖_{2}^{2} \\ = \frac{1}{2} \sum_{i = 1}^{n} ‖ ({\vec{p}}_{i} - \vec{p} - R ({\vec{p}}_{i}^{'} - {\vec{p}}^{'})) ‖_{2}^{2} + ‖ \vec{p} - R {\vec{p}}^{'} - \vec{t} ‖_{2}^{2} \\ + 2 ({\vec{p}}_{i} - \vec{p} - R ({\vec{p}}_{i}^{'} - {\vec{p}}^{'}))^{⊤} (\vec{p} - R {\vec{p}}^{'} - \vec{t}) \end{aligned}

$\begin{aligned} \frac{1}{2}\sum_{i=1}^{n} \|\vec{p}_i-(\mathbf{R}\vec{p}'_i + \vec{t})\|_2^2 &= \frac{1}{2}\sum_{i=1}^{n} \| \vec{p}_i-\mathbf{R}\vec{p}'_i - \vec{t} -\vec{p} + \mathbf{R}\vec{p}' + \vec{p} - \mathbf{R}\vec{p}' \|_2^2 \\ &= \frac{1}{2}\sum_{i=1}^{n} \| (\vec{p}_i -\vec{p} -\mathbf{R}(\vec{p}'_i -\vec{p}')) + (\vec{p} - \mathbf{R}\vec{p}'- \vec{t} ) \|_2^2 \\ &= \frac{1}{2}\sum_{i=1}^{n} \| (\vec{p}_i -\vec{p} -\mathbf{R}(\vec{p}'_i -\vec{p}')) \|_2^2 + \| \vec{p} - \mathbf{R}\vec{p}'- \vec{t} \|_2^2 \\ &~~+ 2(\vec{p}_i -\vec{p} -\mathbf{R}(\vec{p}'_i -\vec{p}'))^\top(\vec{p} - \mathbf{R}\vec{p}'- \vec{t}) \end{aligned}$

上式中， $(\vec{p}_i -\vec{p} -\mathbf{R}(\vec{p}'_i -\vec{p}'))$ 在求和后为0，可以上式可简化为：

min_{R, \vec{t}} J = \frac{1}{2} \sum_{i = 1}^{n} ‖ ({\vec{p}}_{i} - \vec{p} - R ({\vec{p}}_{i}^{'} - {\vec{p}}^{'})) ‖_{2}^{2} + ‖ \vec{p} - R {\vec{p}}^{'} - \vec{t} ‖_{2}^{2}

$\underset{\mathbf{R},\vec{t}} {\min} ~J = \frac{1}{2}\sum_{i=1}^{n} \| (\vec{p}_i -\vec{p} -\mathbf{R}(\vec{p}'_i -\vec{p}')) \|_2^2 + \| \vec{p} - \mathbf{R}\vec{p}'- \vec{t} \|_2^2$
可以看到，左边只和旋转

R

$\mathbf{R}$ 有关，右边有

R

$\mathbf{R}$ 和

\vec{t}

$\vec{t}$ 。所以ICP的求解可拆分为三个步骤：

计算两组点的质心 $\vec{p}$ 和 $\vec{p}'$ ,再计算去质心坐标：

{\vec{q}}_{i} = {\vec{p}}_{i} - \vec{p}, {\vec{q}}_{i}^{'} = {\vec{p}}_{i}^{'} - {\vec{p}}^{'}

$\vec{q}_i = \vec{p}_i -\vec{p}, \vec{q}'_i = \vec{p}'_i - \vec{p}'$
(实际上是上式引入中间变量)
求解优化问题：

R^{*} = \arg min_{R} J = \frac{1}{2} \sum_{i = 1}^{n} ‖ {\vec{q}}_{i} - R {\vec{q}}_{i}^{'} ‖_{2}^{2}

$\mathbf{R}^* = \arg \underset{\mathbf{R}} {\min} ~J = \frac{1}{2}\sum_{i=1}^{n} \| \vec{q}_i -\mathbf{R}\vec{q}'_i \|_2^2$
再根据第二步的

R

$\mathbf{R}$ , 计算

\vec{t}

$\vec{t}$ :

{\vec{t}}^{*} = \vec{p} - R {\vec{p}}^{'}

$\vec{t}^* = \vec{p}-\mathbf{R}\vec{p}'$
展开关于

R

$R$ 的误差项，得到：

\frac{1}{2} \sum_{i = 1}^{n} ‖ {\vec{q}}_{i} - R {\vec{q}}_{i}^{'} ‖_{2}^{2} = \frac{1}{2} \sum_{i = 1}^{n} {\vec{q}}_{i}^{⊤} {\vec{q}}_{i} + {\vec{q}}_{i}^{' ⊤} R^{⊤} R {\vec{q}}_{i}^{'} - 2 {\vec{q}}_{i}^{' ⊤} R {\vec{q}}_{i}^{'}

$\frac{1}{2}\sum_{i=1}^{n} \| \vec{q}_i -\mathbf{R}\vec{q}'_i \|_2^2 =\frac{1}{2}\sum_{i=1}^{n} \vec{q}^\top_i\vec{q}_i + \vec{q}'^\top_i\mathbf{R}^\top\mathbf{R}\vec{q}'_i - 2\vec{q}'^\top_i\mathbf{R}\vec{q}'_i$
显然第一项与

R

$\mathbf{R}$ 无关，

R^{⊤} R = I

$\mathbf{R}^\top\mathbf{R}=\mathbf{I}$ , 第二项亦与

R

$\mathbf{R}$ 无关，所以目标可简化为：

\sum_{i = 1}^{n} - {\vec{q}}_{i}^{' ⊤} R {\vec{q}}_{i}^{'} = \sum_{i = 1}^{n} - t r (R {\vec{q}}_{i}^{'} {\vec{q}}_{i}^{⊤}) = - t r (R \sum_{i = 1}^{n} {\vec{q}}_{i}^{'} {\vec{q}}_{i}^{⊤})

$\sum_{i=1}^{n} - \vec{q}'^\top_i\mathbf{R}\vec{q}'_i =\sum_{i=1}^{n} -tr(\mathbf{R}\vec{q}'_i\vec{q}_i^\top) = -tr\left( \mathbf{R}\sum_{i=1}^{n}\vec{q}'_i\vec{q}^\top_i \right)$
定义矩阵:

W = \sum_{i = 1}^{n} {\vec{q}}_{i} {\vec{q}}^{'}_{i}^{⊤}

$\mathbf{W}=\sum_{i=1}^n \vec{q}_i {\vec{q}'}_i^\top$
所以

W

$\mathbf{W}$ 是一个

3 \times 3

$3\times 3$ 的矩阵，对其进行SVD分解得到：

W = {U Σ V}^{⊤}

$\mathbf{W}=\mathbf{U\Sigma V}^\top$
当

W

$\mathbf{W}$ 为满秩时,

R

$\mathbf{R}$ 为：

R = {U V}^{⊤}

$\mathbf{R}=\mathbf{UV}^\top$
解出

R

$\mathbf{R}$ 后，代入即可求出

\vec{t}

$\vec{t}$ .

非线性的优化方法

非线性优化的思想是用迭代的方式去寻找最优值。当以李代数表达位姿时，目标函数改写为：

min_{ξ} \frac{1}{2} \sum_{i = 1}^{n} ‖ ({\vec{p}}_{i} - \exp (ξ^{\land}) {\vec{p}}_{i}^{'}) ‖_{2}^{2}

$\underset{\xi}{\min}\frac{1}{2}\sum_{i=1}^{n} \| (\vec{p}_i - \exp(\xi^\wedge)\vec{p}'_i) \|^2_2$
它关于位姿导数已推导过，使用李代数扰动模型：

\frac{\partial \vec{e}}{\partial δ ξ} = - {(\exp (ξ^{\land}) {\vec{p}}_{i}^{'})}^{\oplus}

$\frac{\partial \vec{e}}{\partial\delta\xi} = -\left( \exp(\xi^\wedge)\vec{p}'_i \right)^\oplus$

ICP问题存在唯一解和无穷多解的情况
若存在唯一解，极小值点即为全局最优解
在匹配已知的情况下，最小二乘实际上具有解析解，没有必要用迭代优化
可以混合使用PnP和ICP优化
- 深度值已知的点，用建模它们的3D-3D误差
- 深度值未知的点，用建模3D-2D的重投影误差