摘要

3D-3D的位姿估计问题是指，对于空间中的某一点，我们知道这个点在两个相机坐标系中的三维坐标，如何利用这两个三维坐标来求解这两个相机坐标系的运动就是3D-3D的位姿评估问题。

这个问题通常用迭代最近点(Iterative Closest Point,ICP)求解。

假设空间中的一系列点在第一个相机坐标系下的三维坐标为$C={c_1,…,c_n}$，在第二个相机坐标系下匹配的三维坐标为$C’={c_1’,…,c’_n}$。则有：
$\forall i, \ c_i=Rc_i’+t $

对于ICP的求解主要分为两种方式：利用线性代数的求解和利用非线性优化方式求解。

线性代数求解

构造误差项：
$e_i = c_i - (Rc_i’+t) $

将这个误差项构造成一个最小二乘问题：

$\min \limits _{R,t} J= \dfrac{1}{2} \sum \limits _{i=1} ^n \begin{Vmatrix}
c_i - (Rc_i’+t)
\end{Vmatrix} _2 ^2$

通过求解这个最小二乘问题，我们可以得到$R$和$t$。

求解过程真心没看懂啊。

首先计算两组点的质心：
$p=\dfrac{1}{n} \sum \limits _{i=1}^n p_i \\
p’=\dfrac{1}{n} \sum \limits _{i=1}^n p’_i
$

然后计算每个点的去质心坐标：
$q_i = p_i - p \\
q_i’ = p’_i - p’
$

接着构造矩阵：
$W=\sum \limits _{i=1}^n q_i {q’}_i^T $

W是一个3x3的矩阵，对W进行SVD分解，得：
$W=U \Sigma V^T$

当$W$满秩时，$R$为：
$R=UV^T$
求得$R$后，$t$也就可以求出来了。

非线性优化方法通过迭代的方式去寻找最优值。首先要用李代数来表达位姿，即：

$\min \limits _{\xi} J= \dfrac{1}{2} \sum \limits _{i=1} ^n \begin{Vmatrix}
c_i - \exp([\xi]_{\times}c_i’)
\end{Vmatrix} _2 ^2$

然后通过对李代数求导和迭代的方式计算极小值。我们之前讨论的李代数求导终于派上了用场。
而且在这个问题中，已经有理论可以证明ICP问题存在唯一解或无穷多解。在唯一解的情况下，只要能找到极小值解，这个极小值就是全局最优值。