2019年1月11日，港科大VINS-Mono的团队发布了VINS的扩展版本——VINS-Fusion。其支持多种视觉惯性传感器类型（单相机+ IMU，立体相机+ IMU，立体相机），还支持将VINS与GPS进行融合。本人就VINS-Fusion中多传感器局部位姿估计的相关论文进行了阅读，并进行内容整理（尽可能简洁）。
原文：A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors
《一种基于通用优化方法的多传感器局部里程计估计框架》
如今机器人上配备了越来越多的传感器以提高鲁棒性和自主能力。我们看到各种传感器组件配备在不同平台上，例如地面车辆上的立体相机，手机带有IMU（惯性测量单元）和单目相机，以及空中机器人上带有IMU和立体相机。尽管过去已经提出了许多用于状态估计的算法，但是它们通常应用于单个传感器或特定传感器组件。很少可以支持多种传感器。本文提出了一种基于通用优化的里程计估计框架，它支持多个传感器组。在框架中每个传感器都被视为一个通用因子。将共享相同状态变量的因子相加在一起以构建优化问题。我们进一步展示了视觉和惯性传感器的通用性，它们形成三种传感器组件（立体相机，带IMU的单目相机和带IMU的立体相机）。我们在公共数据集和通过多传感器的实际实验验证了该系统的性能。将结果与其他最先进的算法进行比较。我们强调该系统是一个通用框架，可以在位姿图优化中轻松融合各种传感器。我们开源了实现。
提出了多传感器兼容的通用算法必要性：
1、过去的算法通常仅适用于单个传感器或特定传感器套件，无法移植到不同平台。
2、在传感器发生故障时，应移除不工作的传感器，并在系统中快速添加替代传感器。
本文的贡献：
1、一种支持多传感器的基于优化的通用状态估计框架。
2、使用视觉和IMU详细推导状态估计，这些传感器形成不同的传感器套件（立体相机，单目相机+IMU，以及立体相机+IMU）。
3、对公共数据集和实际实验进行系统评估。
4、开源算法
两种多传感器融合方法：
1、基于滤波器
EKF如MSCKF，其维持了几个相机位姿并利用多相机视图来形成多约束更新。
基于滤波器的方法产生非线性误差，能观性约束的EKF被提出用于提高准确性和一致性。而UKF是不分析雅可比矩阵的EKF扩展。
基于滤波器的方法对时间同步很敏感。任何延迟的测量都会引起麻烦，因为状态无法在滤波过程中传播回来。因此需要特殊的排序机制来确保来自多传感器的所有测量都是有序的。
2、基于优化
通过BA来维持大量测量值并一次优化多个变量，与滤波器方法相比，在时间同步上不敏感，在相同计算复杂性的精度上也较优。
发展出增量求解器以限制位姿图大小，如iSAM；滑动窗口法，保持固定大小的位姿图，将先前的状态被边缘化为先前因子而没有信息丢失。
本文采用了基于滑动窗口优化的状态估计框架。
位姿图中每个节点代表状态，每条边代表测量值得出的因子。构建位姿图，优化它等价于找到尽可能匹配所有边缘的节点配置。
1、问题定义
1）待估计的状态变量：
p和R分别代表世界坐标系下的本体位置向量与旋转矩阵，xcam为相机相关状态，包括每个观察到的特征点深度，ximu是IMU相关状态，包括速度、加速度偏置和陀螺仪偏置。相机到IMU的变换矩阵通过离线标定。定义IMU作为本体坐标系中心（若不使用IMU则定义为左相机）。
2）代价函数
状态估计是一个最大似然估计问题（MLE），假设所有测量值独立且服从高斯分布，可以转变成最小二乘问题，通过BA进行求解：
2、传感器因子
1）相机因子：
对于每个相机帧，检测角点特征并通过KLT光流追踪先前帧。对于双目相机，追踪器也匹配左右图像的特征。
相机因子是重投影过程：将第一次观察的特征重投影到后续帧中。假设特征l在图像帧i被第一次观察，在图像帧t中观察到的残差被定义为（双目同理）：
其中
[
u
i
l
,
v
i
l
]
[u^l_i,v^l_i]
[uil,vil]为在图像i中特征l的第一次观测
[
u
t
l
,
v
t
l
]
[u^l_t,v^l_t]
[utl,vtl]为在图像t中的相同特征的观测
π
c
和
π
c
−
1
\pi_c和\pi^{-1}_c
πc和πc−1为相机模型的投影、逆投影函数
T是4x4齐次变换矩阵
T
b
c
T^c_b
Tbc为从本体坐标系到相机坐标系的外参矩阵（已离线标定）
Ω
t
l
\Omega^l_t
Ωtl是重投影误差的协方差矩阵，是像素坐标系中的定值，来自相机内参标定结果。
2）IMU因子
假设加速度和陀螺仪噪声是高斯白噪声，加速度和陀螺仪偏置是一个随机游走，其导数是高斯白噪声。使用协方差传递函数在流形上对IMU测量值做预积分，IMU残差为：
3）其他因子
可以将其他测量值（轮式里程计、雷达、激光）建模为一般残差因子，并将残差因子加入到代价函数中。
3、优化
采用G-N或L-M方法解最小二乘问题。VINS采用ceres solver实现。
4、边缘化
状态数量随时间增加，计算复杂度平方增长，需要在不丢失有用信息的条件下采用边缘化。边缘化将先前测量转换为保留过去信息的先前项。
被边缘化的状态集合表示为Xm，剩余状态集合表示为Xr。 通过对所有边缘化因子求和，我们得到一个新的H和b。 重新排列顺序后得到：
通过舒尔补进行边缘化，得到新的预先值Hp和bp：
5、讨论
系统是一个总体框架，通过将各种传感器转变为一般残差因子，可以将可以将其轻松添加到系统中。 能够处理传感器故障情况： 当传感器发生故障时，只需删除非不工作的传感器因子，并添加其他替代传感器的新因子。
采用EuRoC MAV Visual-Inertial Datasets进行评估，并与OKVIS进行对比。评估内容包括：RPE (Relative Pose Errors) and ATE (Absolute Trajectory Errors)。
1、纯双目在V1 03 difficult和V2 03 difficult序列跟踪失败，而采用IMU的方法在所有图像序列中成功工作。说明当视觉轨迹因照明变化、无纹理区域或运动模糊而跟踪失败时，IMU可以显著改善运动跟踪性能。
2、纯双目的精度表现最差。而使用IMU显著利于状态估计。 因为IMU测量重力矢量，可以有效地抑制翻滚角和俯仰角的漂移。
3、具有IMU的立体相机并不总是表现最佳，因为它需要比具有IMU的单目摄像机更精确的标定。 不准确的内参和外参标定将在系统中引入更多噪声。
4、多传感器融合增加了系统的稳健性。
提出了一种基于优化的局部位姿估计框架。 该框架可支持多传感器组合，文中演示了3种传感器组件：立体相机、带IMU的单目相机和带IMU的立体相机，也可以推广到其他传感器。并在公共数据集和实际实验中使用多传感器验证了系统性能。
未来将使用全局传感器（例如GPS）扩展该框架，以实现局部准确和全局感知的位姿估计。
本文在对相机和IMU的状态估计推导和之前的VINS论文保持一致，也并没有深入介绍。其创新点在于将VINS构建为一种通用的VIO算法，通过将传感器测量值转换为一般残差因子添加到总代价函数中，以适用不同传感器组件的情况，便于算法在不同机器人上的移植和文中提到的对传感器进行替换。
