Fast and Globally Convergent Pose Estimation from Video Images

Fast and Globally Convergent Pose Estimation from Video Images

abstract

问题：3D几何到2D图片间刚体变换
过去的方法：迭代优化，不能证明收敛，旋转矩阵不保证正交结构
提出方法：最小化object space collinearity error 物体空间共线性误差
accurate
robust to outliers

introduction

PnP问题可以通过3对或4对点，解四次或五次多项式求解。对噪声和outliers敏感。
超过4对点，不存在封闭解。构建非线性最小二乘，通过非线性优化算法求解。
非线性优化算法：GN。需要一个好的初始值猜测，收敛到正确的解。GN不能保证收敛到正确的解。

放宽旋转矩阵的正交限制。简化的perspective相机模型。
迭代下降：

1. R的线性解（无约束）
2. 寻找最接近的正交矩阵
   不保证能收敛到正确的值

灵感来源[28]：他们同时计算观测点的object pose和深度。非线性被深度变量消除。迭代次数多，慢。
提出方法：最小化物体空间共线性误差。
连续提高旋转部分的估计，然后估计相应的translation（平移？变换？本质一样）。
每次迭代求最好的正交解。
正交约束，SVD强制满足。
胜过LM

problem formulation

理想针孔相机模型：v_i，p_i，投影中心，三点共线。

classical iterative methods

最小化重投影误差：假设图像坐标点受高斯噪声干扰，旋转矩阵R通常用**欧拉角（Euler angles）**表示。
GN、LM，解非线性最小二乘问题。
GN：
使用一阶泰勒线性化
需要良好的初始值：translation vector尺度在10%以内，每个旋转角相差15°以内。
LM（非线性最小二乘的标准技术）：
interpolation of steepest descent and GN 最速下降插值 和 高斯牛顿
当前值离正确值远——最速下降：慢，保证收敛（不保证正确？）
当前值离正确值近——高斯牛顿

why another iterative algorithm

经典迭代方法解决，当观测数据带有噪声，且需要高精度的位姿估计。
旋转矩阵欧拉角参数化模糊了这个问题的代数结构。？？
精确，鲁棒，高效

the orthogonal iteration algorithm

optimal absolute orientation solution

orthogonal iteration(OI)使用object space error定义位姿估计，这个函数可以基于3D-3D位姿估计解法或absolute orientation迭代。该算法非常依赖于absolute orientation的解。
$q_i$是相机坐标，$p_i$是世界坐标。
$$q_i=R\cdot p_i+t$$

最小二乘问题，绝对方向，这里不是重投影3D-3D
$$\underset{R,t}{\min }{\Sigma }_{i=1}^n||R{p}_i+t-q_i|{|}^2$$
可以使用四元数得到封闭解？？？[36][37]
或使用SVD（SLAM14讲 ICP - SVD解法）：互协方差矩阵SVD（不唯一），但能得到最优R。

the algorithm

object space error:将计算值投影到观测值向量
$$e_i=(I-\widehat{V_i})(R{p}_i+t)$$
$\widehat{V_i}$是line-of-sight projection matrix
最小化平方误差：
$$E(R,t)={\Sigma }_{i=1}^n||e_i|{|}^2$$

3.4 Initialization and Weak Perspective Approximation

weak perspective算法得到初始旋转矩阵$R^{(0)}$
weak perspective:

1. 所有的相机空间坐标都大概等于principle depth(s)
2. object离相机的光轴较近

3.5 Depth-Dependent Noise

OI算法的代价是当图像受到homogeneous 高斯噪声时会有偏差。这是因为，当参考点远离相机的时候object-space共线性误差会增加。