📌 多传感器融合有两个趋势：
📌 算法框架图：
位姿图的示意图如下所示。每个节点表示某一时刻的状态（位置、方向、速度等）。每条边代表一个因子，该因子由一次测量得出。因素限制一种状态、两种状态或多种状态。对于 IMU 因子，它通过连续运动限制来约束两个连续状态。对于视觉地标，它的因子限制了多个状态，因为它是在多个帧上观察到的。一旦图建立起来，优化它就等于找到尽可能匹配所有边的节点的配置。
在本文中，我们专门演示了带有视觉和惯性传感器的系统。视觉和惯性传感器可以形成三种组合进行 6-DoF 状态估计，分别是立体相机、带有 IMU 的单目相机和带有 IMU 的立体相机。所提出的具有视觉和惯性传感器的框架的图形说明如图 3 所示。位姿图中存在几个相机位姿、IMU 测量和视觉测量。 IMU 和其中一个摄像头是可选的。
📌 1). 状态变量：
2). Cost Function:
状态估计的本质是一个 MLE（最大似然估计）问题。 MLE 由一段时间内机器人姿态的联合概率分布组成。在所有测量都是独立的假设下，问题通常被导出为，
其中 S 是一组测量值，来自相机、IMU 和其他传感器。我们假设测量的不确定性是高斯分布的，p(zkt |X ) ∼ N ( ̄ zkt , Ωkt )。因此，上述方程的负对数似然写为，
马氏范数定义为 ∣ ∣ r ∣ ∣ Ω 2 = r T Ω − 1 r ||r||^2_{\Omega}=r^T\Omega^{-1}r ∣∣r∣∣Ω2=rTΩ−1r。 h(·) 是传感器模型，将在下一节中详细介绍。然后将状态估计转换为非线性最小二乘问题，也称为捆绑调整（BA）。
📌 1). Camera Factor：
[ u i l v i l ] \begin{bmatrix} u^l_i & v^l_i\end{bmatrix} [uilvil]是 i i i图像中的 l l l特征第一次观测的像素坐标
[ u t l v t l ] \begin{bmatrix} u^l_t & v^l_t\end{bmatrix} [utlvtl]是 t t t图像中的对同样特征 l l l特征观测的像素坐标
π c \pi_c πc和 π c − 1 \pi^{-1}_c πc−1 是依赖于相机模型（针孔、全向或其他模型）的投影和反投影函数。
T
T
T是
4
∗
4
4*4
4∗4齐次变换，即
[
R
p
0
1
]
\begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}
[R0p1]
T
b
c
T^c_b
Tbc是从b系到相机c系的外参,需要离线标定
T
i
T_i
Ti是
i
i
i图像时刻
b
b
b系相对于
w
w
w系的转换矩阵
T
b
i
w
T^w_{b_i}
Tbiw
T
t
T_t
Tt是
t
t
t图像时刻
b
b
b系相对于
w
w
w系的转换矩阵
T
b
t
w
T^w_{b_t}
Tbtw
重投影误差的协方差矩阵
Ω
t
l
\Omega^l_t
Ωtl是像素坐标中的一个常数值，它来自相机的内在校准结果。
这个因素对于左摄像头和右摄像头都是通用的。我们可以将左图像的特征投影到时间空间的左图像，也可以将左图像的特征投影到空间空间的右图像。对于不同的相机，应使用不同的外在变换
T
b
c
T^c_b
Tbc。
2). IMU Factor:
其中圈减是流形上的减法运算，专门用于非线性旋转。 dt 是两个时间点之间的时间间隔。 g 是已知的重力矢量，其范数约为 9.81。每两个相邻的帧在成本函数中构成一个 IMU 因子。
📌 在传统中，方程 3 的非线性最小二乘问题通过 Newton-Gaussian 或 Levenberg-Marquardt 方法求解。成本函数相对于状态的初始猜测 X ^ \hat X X^线性化。那么，代价函数等于：
J J J是各个因子对当前状态向量 X ^ \hat X X^的雅可比矩阵
我们以 NewtonGaussian 为例，解推导如下，
最后，当前状态 ^ X 用 ^ X ⊕ δX 更新，其中 ⊕ 是对流形进行旋转的加操作。这个过程迭代几次直到收敛。
📌 由于状态的数量随着时间的推移而增加，因此计算复杂度将相应地二次增加。为了限制计算复杂度，在不丢失有用信息的情况下加入了边缘化。边缘化过程将先前的测量转换为先前的术语，该术语保留了过去的信息。被边缘化的状态集记为
X
m
X_m
Xm，剩余状态集记为
X
r
X_r
Xr。通过将所有边缘化因素（eq.7）相加，我们得到一个新的
H
H
H和
b
b
b。重新排列状态顺序后，我们得到以下关系：
[
H
m
m
H
m
r
H
r
m
H
r
r
]
[
δ
X
m
δ
X
r
]
=
[
b
m
b
r
]
\begin{bmatrix} H_{mm} & H_{mr} \\ H_{rm} & H_{rr} \end{bmatrix} \begin{bmatrix} \delta X_m \\ \delta X_r \end{bmatrix}=\begin{bmatrix}b_m\\b_r \end{bmatrix}
[HmmHrmHmrHrr][δXmδXr]=[bmbr]
使用 Schur补 [25] 进行边缘化，如下所示：
我们得到剩余状态的新先验 H p , b p H_p,b_p Hp,bp
有关边缘化状态的信息被转换为先前项而没有任何损失。具体来说，我们在系统中保留了十个空间相机帧。当一个新的关键帧到来时，我们将与第一帧的状态相关的视觉和惯性因素边缘化。
在我们获得有关当前状态的先验信息后，使用贝叶斯规则，我们可以将后验计算为似然和先验的乘积：p(X |z) ∝ p(z|X )p(X)。然后状态估计变成了 MAP（最大后验概率）问题。表示我们在滑动窗口中保持从时刻 m 到时刻 n 的状态。 m之前的状态被边缘化并转换为前项。因此，MAP问题写成：
与方程3相比，上述方程只增加了一个先验项。 Ceres 求解器 [24] 求解与 eq.3 相同。
