A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors论文翻译整理

综述部分

📌 多传感器融合有两个趋势：

1. 基于滤波的融合（MSCKF、EKF、UKF）
2. 基于优化的滤波（BA）
   基于滤波器的方法对时间同步很敏感。任何迟来的测量都会引起麻烦，因为状态不能在过滤过程中传播回来。因此，需要特殊的排序机制来确保来自多个传感器的所有测量都是有序的。
   与基于滤波器的方法相比，基于优化的方法在时间同步方面具有优势，可以处理来自多个传感器的测量信息。基于优化的算法在准确性方面也优于基于滤波器的算法，但代价是计算复杂性。早期的优化求解器，例如 G2O [19]，利用 Gauss-Newton 和 Levenberg-Marquardt 方法来解决问题。尽管在优化求解器中采用了稀疏结构，但复杂度随着状态和测量的数量呈二次方增长。为了实现实时性能，一些算法探索了增量求解器，而另一些则限制了位姿图的大小。 iSAM2 [20] 是一种高效的增量求解器，当新的测量出现时，它重用了先前的优化结果以减少计算量。优化迭代只更新了一小部分状态而不是整个位姿图。随后，在[21]中提出了一种加速求解器，通过将密集结构重构为稀疏块来提高效率。保持固定大小的位姿图的方法称为滑动窗口方法。令人印象深刻的基于优化的 VIO 方法，例如 [8, 10, 12]，在有界大小的滑动窗口上优化了变量。在 [8, 12] 中，先前的状态被边缘化为先验因素而没有丢失信息。

算法部分

SYSTEM OVERVIEW

📌 算法框架图：

位姿图的示意图如下所示。每个节点表示某一时刻的状态（位置、方向、速度等）。每条边代表一个因子，该因子由一次测量得出。因素限制一种状态、两种状态或多种状态。对于 IMU 因子，它通过连续运动限制来约束两个连续状态。对于视觉地标，它的因子限制了多个状态，因为它是在多个帧上观察到的。一旦图建立起来，优化它就等于找到尽可能匹配所有边的节点的配置。

在本文中，我们专门演示了带有视觉和惯性传感器的系统。视觉和惯性传感器可以形成三种组合进行 6-DoF 状态估计，分别是立体相机、带有 IMU 的单目相机和带有 IMU 的立体相机。所提出的具有视觉和惯性传感器的框架的图形说明如图 3 所示。位姿图中存在几个相机位姿、IMU 测量和视觉测量。 IMU 和其中一个摄像头是可选的。

METHODOLOGY

A. Problem Definition

📌 1). 状态变量：

2). Cost Function:
状态估计的本质是一个 MLE（最大似然估计）问题。 MLE 由一段时间内机器人姿态的联合概率分布组成。在所有测量都是独立的假设下，问题通常被导出为，

其中 S 是一组测量值，来自相机、IMU 和其他传感器。我们假设测量的不确定性是高斯分布的，p(zkt |X ) ∼ N ( ̄ zkt , Ωkt )。因此，上述方程的负对数似然写为，

马氏范数定义为$||r|{|}_{\Omega }^2=r^T{\Omega }^{-1}r$。 h(·) 是传感器模型，将在下一节中详细介绍。然后将状态估计转换为非线性最小二乘问题，也称为捆绑调整（BA）。

B. Sensor Factors

📌 1). Camera Factor：

$\left[\begin{array}{cc}{u}_i^l& v_i^l\end{array}\right]$是$i$图像中的$l$特征第一次观测的像素坐标

$\left[\begin{array}{cc}{u}_t^l& v_t^l\end{array}\right]$是$t$图像中的对同样特征$l$特征观测的像素坐标

${\pi }_c$和 ${\pi }_c^{-1}$ 是依赖于相机模型（针孔、全向或其他模型）的投影和反投影函数。

$T$是$4\ast 4$齐次变换，即$\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$
$T_b^c$是从b系到相机c系的外参,需要离线标定
$T_i$是$i$图像时刻$b$系相对于$w$系的转换矩阵$T_{b_i}^w$

$T_t$是$t$图像时刻$b$系相对于$w$系的转换矩阵$T_{b_t}^w$
重投影误差的协方差矩阵${\Omega }_t^l$是像素坐标中的一个常数值，它来自相机的内在校准结果。
这个因素对于左摄像头和右摄像头都是通用的。我们可以将左图像的特征投影到时间空间的左图像，也可以将左图像的特征投影到空间空间的右图像。对于不同的相机，应使用不同的外在变换$T_b^c$。
2). IMU Factor:

其中圈减是流形上的减法运算，专门用于非线性旋转。 dt 是两个时间点之间的时间间隔。 g 是已知的重力矢量，其范数约为 9.81。每两个相邻的帧在成本函数中构成一个 IMU 因子。

C. Optimization

📌 在传统中，方程 3 的非线性最小二乘问题通过 Newton-Gaussian 或 Levenberg-Marquardt 方法求解。成本函数相对于状态的初始猜测$\hat{X}$线性化。那么，代价函数等于：

$J$是各个因子对当前状态向量$\hat{X}$的雅可比矩阵

我们以 NewtonGaussian 为例，解推导如下，

最后，当前状态 ^ X 用 ^ X ⊕ δX 更新，其中 ⊕ 是对流形进行旋转的加操作。这个过程迭代几次直到收敛。

D. Marginalization

📌 由于状态的数量随着时间的推移而增加，因此计算复杂度将相应地二次增加。为了限制计算复杂度，在不丢失有用信息的情况下加入了边缘化。边缘化过程将先前的测量转换为先前的术语，该术语保留了过去的信息。被边缘化的状态集记为$X_m$，剩余状态集记为$X_r$。通过将所有边缘化因素（eq.7）相加，我们得到一个新的$H$和$b$。重新排列状态顺序后，我们得到以下关系：
$\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]\left[\begin{array}{c}\delta X_m\\ \delta X_r\end{array}\right]=\left[\begin{array}{c}{b}_m\\ b_r\end{array}\right]$
使用 Schur补 [25] 进行边缘化，如下所示：

我们得到剩余状态的新先验$H_p,b_p$

有关边缘化状态的信息被转换为先前项而没有任何损失。具体来说，我们在系统中保留了十个空间相机帧。当一个新的关键帧到来时，我们将与第一帧的状态相关的视觉和惯性因素边缘化。

在我们获得有关当前状态的先验信息后，使用贝叶斯规则，我们可以将后验计算为似然和先验的乘积：p(X |z) ∝ p(z|X )p(X)。然后状态估计变成了 MAP（最大后验概率）问题。表示我们在滑动窗口中保持从时刻 m 到时刻 n 的状态。 m之前的状态被边缘化并转换为前项。因此，MAP问题写成：

与方程3相比，上述方程只增加了一个先验项。 Ceres 求解器 [24] 求解与 eq.3 相同。