Visual-Inertial Monocular SLAM with Map Reuse
重用地图的单目视觉惯导SLAM系统
摘要
近些年来有很多优秀的视觉惯导融合的里程计系统，计算高精度和鲁棒性的传感器的增量运动。但是这些系统都是没有闭环的，所以导致系统即使回到观测过的地方还是会有累计误差。本文作者提出了一个新颖的基于紧耦合的带有闭环检测的视觉惯导SLAM系统，他可以在已经建图的地方重用地图达到0漂移的定位精度。这个系统可以用在所有的相机上，这里主要介绍存在尺度不确定性的单目相机。本文也提出了一个新颖的IMU初始化的方法可以在短时间内计算很高精度的尺度，重力方向，速度，加速度计和陀螺仪的偏置。在11个序列的飞行数据集上进行了测试，尺度误差达到1%（厘米级）精度。
本文未开源，但是有大佬复现的代码：https://github.com/jingpang/LearnVIORB
预备知识
系统的输入是IMU的测量和单目相机帧，利用针孔模型利用投影模型可以把相机坐标系下的3D点投影到2D的图像平面：
需要注意的是这个投影方程并没有考虑相机的畸变，当我们检测到了图像中的特征点先要对他们去畸变。
公式解释：IMU的测量全部都在B系下，公式1IMU的角度测量减去偏置得到角速度真值，再和时间相乘得到时间间隔(K到K 1帧的)内的角度变化量，再加一个Exp相当于变换到了李群的空间，和上一帧的位姿相乘得到当前帧在W系下的位姿。公式2利用当前加速度的测量量减去当前帧的偏置和时间相乘得到速度变换量再转换到世界坐标系下，再加上重力加速度引起的变化和上一帧的速度相加得到当前帧的速度。公式3当前帧的位置其实就是匀加速运动的位移方程。
视觉惯导的ORB-SLAM
这篇论文是基于ORB-SLAM做的，对于ORB-SLAM的介绍，可以参考解析ORB-SLAM3的文章。下边主要介绍加上IMU后对ORB-SLAM框架中Tracking、Local Mapping和Loop Closing的影响。
Tracking
主要的变化是添加了相机频率的IMU位置、速度和偏置的估计。这样我们就可以得到比恒速模型准的多的机器人当前帧的初始状态。一旦预测到了机器人的位置，局部地图中的地图点就投影到当前帧和当前帧的特征点匹配。然后优化当前帧中特征匹配的重投影误差和IMU的误差。这种优化是不同的，取决于地图是否被局部建图或闭环线程更新，如图2所示。
当Tracking在地图更新后执行的时候，IMU的误差连接了当前帧j和上一帧i：
公式解释：这里就是重投影误差的最小二乘的形式，利用IMU当前帧的位姿得到世界坐标系下地图点在B系下的表示，然后利用外参可以得到地图点在C系下的表示，最后利用针孔模型得到该地图点在当前帧像素坐标系下的表示，得到和匹配点的误差。
B.局部建图
在一个新的关键帧插入的时候局部建图线程进行Local BA，他优化最新的N帧(Local window)和所有被观测到的地图点，其他共视图中维护的不在局部滑窗中的关键帧虽然参与优化但是不改变位姿。固定窗口中包含了N 1个和滑窗相连的最新的共视关键帧，他们可以约束IMU的状态。
上图表示出了ORB-SLAM中的BA和ORB-SLAM-VI中BA的区别，这里的优化函数是IMU的误差和重投影误差的和，和纯视觉的方法相比每个关键帧多了九个状态量（速度和偏置）需要优化。所以要选择一个合适的窗口的大小来实时的优化。
局部建图线程还负责关键帧的管理。原始的ORB-SLAM策略中会丢弃冗余的关键帧，这样就不会在已经建图很完善的地方增加很多的关键帧。但是这种方法与IMU约束连续关键帧的机制是不匹配的，两个连续关键帧的时间相差越长，IMU提供的信息就越弱。因此如果局部窗口中的关键帧没有差过0.5s的我们允许局部建图线程丢弃冗余关键帧。为了能在闭环检测或者其他时间优化地图，本文不允许两个关键帧相差3s。如果关闭具有IMU约束的完整BA，只需要约束局部窗口中的时间偏移即可。
C.闭环检测
闭环的主要作用是机器人又回到原来到过的地方的时候降低里程计带来的累计误差。场景充实别模块匹配最近的关键帧和原来的一个关键帧。利用两帧的词袋匹配可以得到刚体变换，然后执行优化的过程来降低里程计带来的累计误差。这种优化在大场景中很耗时，所以执行位姿图优化，==这样就可以忽略结构==，也可以很好的收敛。本文可以执行六自由度的位姿图优化，因为尺度是可观的。这个系统忽略IMU信息不优化速度和偏置。==速度利用把关联的关键帧旋转到正确的位姿上来修正。==同时并行的执行一个全局的BA来优化所有的状态，包括速度和偏置。
IMU初始化
根据由运行一段时间的单目SLAM算法得到的若干关键帧来计算视觉-惯导的全局BA的尺度、重力方向、速度和IMU偏置的初始估计。运行一段时间（几秒钟）的单目SLAM算法，假设传感器运动后导致所有的状态都可观。
初始化问题可以分成小的子问题
- 陀螺仪偏置
- 尺度和重力估值（假设没加速度偏置）
- 加速度偏置估计，尺度和重力方向细化
- 速度估计
陀螺仪偏置估计
可以利用两个连续关键帧的orientation估计陀螺仪的偏置，假设偏差的变化忽略不计（bias是缓慢变化的量），可以直接对所有的连续关键帧优化常数的bg（最小化陀螺仪的积分和ORB-SLAM计算出来的orientation）：
尺度和重力估计（没有加速度的偏置）
得到陀螺仪的偏置以后，可以利用预积分得到速度，位置，正确的旋转加速度的测量来补偿陀螺仪的bias。ORB-SLAM计算的相机轨迹具有尺度不确定性，所以在从相机坐标系到IMU的机体坐标系转的时候需要加一个尺度因子：(imu的所有的状态都是在B系下测量得到的)
C.加速度偏置估计，尺度和重力细化
同样可以整理为公式12的形式：
D.速度估计
在公式12和19中都是考虑三个连续的关键帧，所以线性的系统中就没有3N个额外的未知的速度，现在所有的速度都可以用公式18在重力、尺度和偏置已知的情况下来计算。为了计算最近几帧的速度，这里用公式3。
E.重定位后的bias重新初始化
当系统重新初始化的时候，利用公式9重新初始化陀螺仪偏置，利用公式19重新初始化加速度计偏置，尺度和重力是已知的。利用二十个关键帧来初始化这两个偏置。
备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区
本文仅做学术分享，如有侵权，请联系删文。
