《增强现实：原理、算法与应用》读书笔记（10）视觉惯性SLAM

《增强现实：原理、算法与应用》读书笔记（10）视觉惯性SLAM

单目视觉SLAM系统存在一定的局限性，它非常依赖相机的成像质量，在图像质量不佳的时候则难以正常工作。即使图像质量很好，缺乏尺度信息也对重建造成了困难。而且在AR应用中，为了与场景交互，SLAM算法必须提供鲁棒的带正确尺度信息的相机位姿估计。而随时可能出现的光照、纹理质量的变化，以及相机快速运动带来的图像模糊，都给SLAM带来很大的挑战。

为了解决这些问题，我们可以考虑引入其他辅助手段，比如使用更好的图像特征、更鲁棒的特征匹配方法来减轻图像质量不佳造成的影响，借助场景中尺度已知的物体和标志（marker），或是采用多目视觉系统来恢复尺度信息。

所以我们可以引入一种新的传感器——惯性测量单元（inertial measurement unit，IMU）来解决这些问题。本节将会介绍一类基于视觉和惯性测量单元的SLAM系统（下文简称VISLAM）。在IMU的帮助下，提高相机跟踪算法的鲁棒性，同时得到场景的准确尺度信息。

IMU基本模型

惯性测量单元是测量物体三轴角速度以及加速度的装置，其工作极其稳定。通常IMU能以远高于相机的频率（数百到上千赫兹）得到相对于自身坐标系的角速度和加速度。依靠这些数据，可通过计算得到系统的旋转、速度和平移等信息。

很自然地，由于IMU工作并不依赖于视觉信息，可以在恶劣的光照、纹理以及模糊图像的条件下为我们提供可靠的跟踪结果。并且，由于整个系统的速度$v$和平移$p$分别来自于加速度$a$的一次和二次数值积分，因而基于IMU的跟踪结果直接带有了场景的尺度信息。

1）噪声模型

正确地使用IMU并不是一件容易的事，和所有传感器一样，IMU信号也带有误差，不管是使用滤波的方法还是非线性优化的方法处理IMU信息，首先都需要对其进行合理的建模。我们可以将IMU的输出数据（IMU对实际运动的观测）和输入数据（实际的物理运动）的差异定义为IMU读数的误差，他可能包含偏移、随机游走噪声、尺度系数、不正交性等。

IMU误差的成分比较复杂，不同类型误差对于运动估计的影响也是不同的。在位姿估计的过程中，对角速度$\omega$数值积分得到系统的朝向$R$，而对加速度$a$进行一次和二次积分，分别得到速度$v$和平移$p$。加速度经过了两次数值积分，其误差也会通过两次积分进入平移量$p$中，故相比角速度的误差，我们的跟踪算法对于加速度的误差更为敏感。根据实际经验得到，由IMU积分得到的朝向信息较为准确，而位置信息误差则是较大的。

以常用的六轴IMU为例，目前的视觉惯性里程计算法（visual inertial odometry，VIO）中最为常用的做法是将其误差模型简化为偏移和测量噪声两个部分，并假设加速度计和陀螺仪相互统计独立，且各自的三轴之间都是统计独立的。我们用$\sim$标记每次观测的值，不带$\sim$的表示真实值，在不考虑地球自转的情况下，角速度和加速度的观测量分别为（Mourikis et al., 2007）：

$$\tilde{\omega }=\omega +b^g+{\eta }^g$$$$\tilde{a}{=}^G{\text{R}}^{\top }\left({}^G\text{a}{-}^G\text{g}\right)+b^a+{\eta }^a$$

使用左上标$G$来表示全局坐标系，不带左上标的表示当前的局部坐标系，陀螺仪的相关参数用右上标$g$表示，加速度计的相关参数用右上标$a$表示。可以看到，IMU的读数可以看成是真实值、偏移（包括角速度偏移$b^g$和加速度偏移$b^a$）、加性高斯白噪声（包括角速度白噪声${\eta }^g$和加速度白噪声${\eta }^a$）之和。加速度计特殊的一点是，它的读数里面还包含重力${}^G\text{g}$的影响，另外就是所谓的偏移和白噪声了：

（1）偏移$b$随时间的增长符合零均值的高斯随机游走。这个噪声从IMU开机以来就一直存在且不断积累，与是否从IMU读取数据无关。可以用一个初始偏移量$b_0$和随机游走噪声$n_b$来描述它：$b=b_0+n_b$。

（2）测量噪声$\eta$为零均值的加性高斯白噪声，表示从IMU读取数据时产生的小抖动，而且这个噪声只在观测它的时候产生。

其中：$$n_{bg}\sim N(0,{\sum }_{bg})$$$${\eta }_{wg}\sim N(0,{\sum }_{wg})$$$$n_{ba}\sim N(0,{\sum }_{ba})$$$${\eta }_{wa}\sim N(0,{\sum }_{wa})$$

故它们的协方差矩阵为：
$$Q_{IMU}=diag({\sum }_{bg},{\sum }_{wg},{\sum }_{ba},{\sum }_{wa})$$

2）离散时间噪声和连续时间噪声

上面说的噪声模型都是在连续时间系统下考虑的，连续时间系统的高斯协方差（或标准差）也就称为传感器的噪声强度。实际上我们不可能每时每刻都在观测IMU，这是一个离散时间系统。离散时间噪声和连续时间噪声的关系，可以参考文献（Smith et al., 1978）。这里我们只是简单地给出离散时间系统下噪声的协方差。以陀螺仪为例，一般假设三轴的噪声独立同分布而且采样时间间隔为$\Delta t$，那么陀螺仪的两种噪声的离散时间协方差可以用如下公式计算：

$${\sum }_{bgd}={\sum }_{bg}\Delta t$$$${\sum }_{wgd}={\sum }_{wg}\Delta t$$

加速度计的离散噪声协方差${\sum }_{bad}$和${\sum }_{wad}$形式也是类似的。

3）IMU状态传递

IMU状态传递（IMU propagation）指的就是通过将当前时刻IMU的读数在时间上进行积分，来得到下一时刻的IMU状态。这个IMU状态包括IMU的位姿、运动以及噪声等，通过积分操作，IMU读数的信息就被传递到了下一时刻。

理想情况下，IMU读数不存在白噪声，偏移量也不存在随机游走的情况，此时IMU的读数符合：
$$\omega =\tilde{\omega }-b^g$$$$a=\tilde{a}-b^a+g$$

记全局坐标系下IMU的状态为（为保持简洁，省略了转置符号）：
$${\phi }_{IMU}\stackrel{\Delta }{=}\left[{}^G\text{R}{,}^G\text{p}{,}^G\text{v},b^g,b^a\right]$$

具体来说，积分过程包括积分这个IMU状态向量的五个分量。那么，要对IMU状态进行积分，首先要写出IMU状态关于时间的导数：
$$\dot{{}^G\text{R}}{=}^G\text{R}[\tilde{\omega }-b^g{]}_{\times }$$$$\dot{{}^G\text{P}}{=}^G\text{v}$$$$\dot{{}^G\text{v}}{=}^G\text{R}(\tilde{a}-b^a+g)$$$${\dot{b}}^g=0$$$${\dot{b}}^a=0$$

显然，由于偏移$b^g$和$b^a$随时间增长符合零均值的高斯随机游走，故它们关于时间的导数为零。

优化方法

视觉信息和IMU信息相结合可以提高跟踪定位的精度。IMU优化可以分为基于滤波的方法和非线性优化方法。两者的数学模型本质是一样的，都是最大化后验估计，主要区别在于有没有做重新线性化。

滤波方法只做一次线性化，而非线性优化在每次迭代的时候会根据每一轮优化的质量去修正信赖域的大小并重新线性化直到收敛。因此，滤波方法可以看作是非线性优化的一次迭代，理论上精度没有非线性优化的精度高，但因为其实现相对简单、速度快，在实际应用中比较常被采用。

无论基于滤波方法还是非线性优化方法实现的VISLAM系统，对IMU的建模都大同小异，基本可以分为两类：一类是类似于MSCKF（Mourikis et al., 2007）、OKVIS（Leutenegger et al., 2015）那样迭代式的IMU积分；另一类则是使用IMU预积分的方法（Forster et al., 2017; Li et al., 2017）。

1）基于MSCKF的VISLAM

MSCKF全称是多状态约束卡尔曼滤波（multi-state constrains Kalman filter），是较早的关于VISLAM的工作。MSCKF使用了迭代式的IMU积分技术，即通过对IMU读数进行积分，得到作为状态的先验分布，而将图像信息作为观测，通过扩展卡尔曼滤波方法求解状态的后验分布。

MSCKF的状态变量实际也是一个状态窗口，包括最新的IMU状态和保留在窗口内的部分历史相机状态。

MSCKF算法的框架如下：

（1）状态传播。对于IMU读数，使用龙格-库塔法对其进行积分，同时根据噪声参数更新状态的协方差矩阵。

（2）图像注册。每当得到新的图像时，根据当前的最新状态以及IMU-相机外参对状态进行增广，得到状态的先验。并对图像进行处理，提取视觉特征，更新特征跟踪信息等。

（3）状态更新。选择合适的时机，计算卡尔曼增益，对状态进行更新，得到后验状态信息。

MSCKF算法是无结构信息的，也就是只估计相机/IMU的状态，而不估计三维点的状态。每当要进行状态更新时，选取一部分特征，通过边缘化的操作将他们的信息融合到相机/IMU状态中，然后再对状态进行更新。

VISLAM框架

VSLAM的基本框架一般包括初始化、前台跟踪、后台优化（包括局部优化和全局优化）、重定位和回路闭合等。而VISLAM的框架一般和VSLAM差异不大，同样也包含这些模块。不过由于多了IMU这个传感器，各个模块的具体实现上会有所差异。

VISLAM逻辑上分为五个模块：初始化、前台视觉惯性跟踪、后台地图优化、回路检测与闭合、重定位等模块。

1）初始化模块

初始化模块的目标是获取初始的状态变量值。与VSLAM相比，VISLAM的状态估计部分由于状态变量多，需要包括初始的旋转、速度、尺度、IMU偏移量。VINS（Li et al., 2017）和VI-ORB（Mur-Artal et al., 2017b）给出了较为完整的初始化思路，大致策略为使用纯视觉SfM和纯惯性积分对齐的方式计算出初始陀螺仪的偏移量，然后将加速度计的偏移量置零，并使用一个简化的线性VIO问题来计算初始状态的速度、重力以及与SfM结果的相对尺度量。

2）前台视觉惯性跟踪模块

前台视觉惯性跟踪模块首先需要考虑相机与IMU数据的时间对齐。通常IMU的频率要远高于相机获取图像的帧率，因此两种传感器之间的时间戳对齐也是VISLAM前台跟踪一个较为棘手的问题。一般可以采用线性插值的方法，以图像的时间戳为准，对IMU的读数进行插值。

插值法可以得到同步后的两个图像帧之间的一组IMU读数组成的窗口，这样每当接收到图像的时候，可以先对图像进行关键点提取，维护图像之间的匹配关系。IMU方面，可以使用前面提到过积分或预积分技术进行处理。

另外，我们可以使用陀螺仪预测当前相机的旋转或者使用IMU预测当前相机位置，进而预测上一个特征点在下一帧图像上的位置，给相应的图像匹配算法或者光流跟踪方法提供一个很好的初始位置预测，特别是剧烈运动的情况下，该方法更为鲁棒。

同时，也可以借用IMU帮助下的2-Point-RANSAC方法（Troiani et al., 2014）进行特征点剔除。此外，因为可以依靠IMU积分来估计相机的位姿，而且在IMU状态估计准确的情况下短时间内一般误差累积不大，所以在VISLAM中实际上并不需要每帧都进行特征匹配。因此我们可以采取在后台线程每隔若干帧抽取一帧的策略，并采用相对比较耗时的特征提取和匹配方法来加强跟踪的稳定性。

通常需要在前台跟踪模块里先计算好最新状态的初始值，以供后台优化使用。这一部分可以认为是后台优化的“初始化”。VSLAM中可能会使用DLT、P3P、PNP等方法来得到旋转和平移的初值，而在VISLAM中，由于IMU的存在，纯惯性或是视觉惯性结合的方式是更为主流的方法。可以采用直接IMU积分的方式获取新一帧的初始值，抑或更进一步，在前台求解一个小规模的VIO集束调整问题（如两到三个状态），并将求解的结果作为状态的初值放到后台进一步优化。

3）后台地图优化模块

后台地图优化模块可以分为基于局部窗口的优化和全局优化。基于局部窗口的优化可以采用带先验信息的局部窗口优化的方法，在后台维护一个较长的状态窗口，每当有新的状态加入到窗口中时触发一次非线性优化求解。在基于关键帧的VSLAM中，通常只需要根据图像本身来考虑关键帧的筛选，如特征是否丰富、特征分布是否合理、与上一帧是否能形成足够的视差和共视等。在窗口滑动的时候也有不同的做法：顺序滑动、或用最新的状态替换窗口中最新的状态。离开窗口的状态则以边缘化的方式滑出窗口，其结果会作为先验信息加入到后续优化中。

VSLAM中除了关键帧本身的图像质量与关键帧之间的视差，还要考虑IMU的影响。移动端常用的消费级IMU通常只能在较短的时间窗口内保证积分精度，因此选择关键帧时，帧间的IMU窗口不宜过长。

VISLAM的视觉部分要求视差足够，而惯性部分要求时间窗口较短，这就导致了矛盾，AR应用中优势会遇到低速运动情况，此时就需要开发者根据实际情况合理的设置关键帧筛选策略以达到最佳的性能。

4）回路检测与闭合模块

回路检测与闭合模块可以在检测到回路闭合时调用全局集束调整，消除长时间跟踪的积累误差。但是出于性能考虑，全局集束调整不适合频繁调用。一种策略是在检测到回路闭合之后，将匹配到的历史关键帧加入到当前的滑动窗口内，并设置为固定的变量。这样，窗口内的集束调整就成了以历史关键帧为条件的极大似然估计。另一方面，又可以再额外维护一个全局的集束调整模块，定期运行以消除历史关键帧的误差。这样可以节省大量计算，同时又不会引入很大的误差累积。

这个额外的集束调整同样可以采用之前介绍的一些方法来加速，如无结构的位姿图优化、增量式集束调整算法等。

5）重定位模块

重定位可以在跟踪丢失或者极度不稳定的状况下，重新定位之前访问过的场景，重置SLAM系统。由于有IMU的积分，相较于VSLAM，VISLAM通常不容易丢失，而一旦丢失，则说明窗口中的IMU的积分结果已经失去意义，需要丢弃并重新初始化。这个重置的过程，可以看做是一个轻量级的初始化模块，同样它需要先使用纯视觉的SfM方法跟踪一段距离，重新初始化IMU模块。

由于之前已经有过一些稳定跟踪的信息，一般只要跟踪丢失的持续时间不是太久，在重定位之后的重新初始化过程中不需要优化全部参数，类似于陀螺仪的偏移量、SfM尺度和重力之类的信息可以保留之前的优化结果，从而降低问题的复杂度。