Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization
Taylor Guo, 2016年4月13日-12:01 – 2016年8月1日-20：30 @ EJU Shanghai
原文发表于Conference :《 Robotics: Science and Systems (RSS)》, 2013-06-24
Github: https://github.com/ethz-asl/okvis
摘要：
视觉和惯性传感器数据融合越来越普遍，它们在传感器感知特征上互补。大部分的融合策略主要是靠滤波器的设计，视觉机器人最近大量采用非线性优化方法处理SLAM，对这个领域内的性能和计算复杂度进行了大量改进。跟随这一趋势，我们在SLAM技术中，从惯性传感器中读取数据，将视觉测量和惯性传感器紧密结合。惯性传感器的误差用概率方法集成到路标的重投影误差里面，这样就可以用一个合成的非线性函数进行优化。利用“关键帧”这一强有力的方式，我们部分地忽略之前的状态，维护一个有限大小的优化窗口，以确保操作的实时性。对比单纯视觉算法和松耦合的视觉-惯性传感器算法，我们的紧耦合方法被证明精度和鲁棒性非常好。
一 简介：
视觉和惯性传感器测量方法是一种常用方法，用于机器人领域，像位姿估计，视觉里程和SLAM中。图像抓到的场景内容丰富，典型的惯性传感器由陀螺仪和加速度计组成、短距离估计精确，两者互补，成功运用于热气球和汽车导航，如论文6、20和论文14。还有，由于这些传感器在大部分智能手机里面都有，引起了极大的兴趣和研究活动，提供了视觉-惯性传感器SLAM的有效方案。
之前，视觉-惯性传感器的位姿估计是通过滤波器方法，其中用惯性传感器连续测量，关键点测量用于更新。论文14提出了基于EKF实时单目融合方法，论文8提出了单目视觉-惯性传感器滤波方法用于户外长轨迹，采用了惯性传感器到相机的校准，和回路控制。这两个工作印象非常深刻，在运动轨迹上误差低于0.5%。论文9提供了基于滤波方法的视觉-惯性传感器融合的观测结果和校准结果。航向和位置的全局不可观测性，用于参考的初始化位姿的不确定性，是视觉-惯性传感器内在的一种估计问题，主要靠线性方法优化。
论文18是纯视觉SLAM优化方法，在与滤波方法相同计算量的状况下，精度更好。非线性优化方法已经很流行了，它使用一个关键帧的相关稀疏图，这些关键帧与路标关联。
视觉-惯性传感器方法可以分为两类。松耦合系统中，如论文10，惯性传感器测量整合进单独的倾角测量计中，非独立、相对的航向测量整合到立体视觉优化里面。论文20，采用单视觉位姿估计作为数据更新，给到具有非直接IMU数据的EKF方法中。论文15,7中，相对的立体位姿估计整合进一个因子图中，它包含惯性数据，和绝对GPS测量数据。这些方法都不复杂，但忽略了不同传感器的内部相关性。相反，紧耦合方法，充分考虑了所有传感器的状态。为了便于追踪，用于取代滤波方法，论文2，提出了一个固定延时的平滑器，它有一个窗口提供连续的机器人位姿和相对状态，删除规定范围以外的的状态，如论文19。论文16，采用类似的方法，但没有惯性传感器，工作在平面环境。
考虑到视觉-惯性传感器SLAM的精度和鲁棒性，我们倾向于紧耦合的融合方法，可以最大程度地利用感知的信息和非线性估计方法，而不是滤波方法，只是部分优化的线性方法。我们的方法是受到论文17的启发，它建议用将IMU的误差放到batch-optimized SLAM处理（只在初始化时）。我们的方法采用论文2中固定延迟平滑器，它集成了惯性传感器，用一个代价函数重映射误差，为了控制计算复杂度，老的状态会被丢弃。
关于这些工作，我们看到三部分的贡献：
1. 在运动很慢或没有运动的情况下，我们也采用关键帧实例用于无漂移的估计：没有采用连续时间位姿的优化窗口，我们保留关键帧，可能位置相差很远（时间上连续，但是不同位置的关键帧）；保留视觉约束，但仍然会参考IMU。我们对关键帧的相对不确定性的推导可以构造一个位姿图，没有提及全局位姿的不确定性，这采用论文13,RSLAM中的思想方法。
2. 我们提供了一个全概率IMU误差推导模型，包括信息矩阵，关联连续的图像帧并没有在IMU的数据频率上外加状态信息。
3. 在系统级别，我们开发了硬件和算法用于精确实时SLAM，包括鲁棒的关键点匹配和采用惯性信息去除离群点算法。
我们还在第2章B节中介绍了分批视觉SLAM中的惯性传感器误差，接着在第2章C节中介绍实时立体图像处理和关键帧的选择，第2章D节中介绍筛选过程。最后，第3章介绍了立体视觉和惯性传感器室内室外的测试结果。
二 紧耦合视觉与惯性传感器融合：
视觉SLAM中，非线性优化通过最小化相机图像帧中观测的路标的重投影误差寻找相机位姿和路标位置。图2是图状表示：方形表示量测数据作为边，圆形节点表示估计量。一旦产生惯性测量数据，它们不仅仅在连续位姿间建立约束，也在连续的速度和IMU漂移估计上建立约束，加速度和陀螺仪也提供了机器人的状态向量。这一章，我们将惯性测量数据整合到分批视觉SLAM中。
图2. 状态变量和测量的图。左边是视觉SLAM，右边是视觉惯性传感器SLAM。
A. 名词和定义
1) 名词：
表示一个参考帧A；表达式中的向量记为pA或pABC，其中B和C作为开始和结束点。图像帧之间的变换矩阵TAB为齐次变换矩阵，它将齐次点的坐标表示从 转变成 。它的旋转矩阵部分记为CAB；对应的四位数就写为 ，ε和η分别表示实部和虚部。我们采用论文1中介绍的名词：关于四位数乘法 ，我们引入左手复合运算 和右手运算符 ，那么 。
2) 坐标系
我们采用图3中描述的一种立体相机/惯性传感器方法评估它的性能。在跟踪相机内部，有一个针对惯性帧的相对表示为 ，我们将相机坐标记为 ，惯性传感器坐标记为 。
图3. 硬件坐标系表示：两个相机作为立体相机有各自坐标， 。惯性传感器数据表示为 。 可以根据世界坐标 来计算。
3) 状态
计算出的变量组成机器人在连续图像时间XkR的状态和路标XcL。XR是机器人在惯性坐标系Pwsw下的位置，机器人航向四位数qws，惯性坐标系下的速度vwsw，陀螺仪的偏移bg，加速度计的偏移ba。因此，XR记为 。还有，位姿状态记为 ，速度和偏移记为 。路标用齐次坐标如论文3中那样表示，用于无缝集成近处和远处的路标： 。
我们使用状态复制出现的切向空间g的摄动方程，采用群操作符田 ，指数操作符exp和对数操作符log。那么，可以定义估计值 的摄动方程为 。我们采用最小坐标表示 。一个从最小坐标岛切向空间的双映射 投影变换为： 。
具体的，我们采用最小化（3D）航向δα∈R3的轴角度摄动方程，航向通过幂映射转换成四元数δq：
因此，可以采用群运算符×，记为 。获取最小的机器人误差状态向量：
与机器人状态分解XT和Xsb类似，我们使用位姿误差状态 和速度/偏移误差状态 。
我们将齐次路标视为非单位四位数，采用最小摄动δβ，因此，δxL:=δβ。
B. 包含惯性变量的分批视觉SLAM
我们通过联立带有权重的重摄影误差er和IMU暂时误差es形成代价函数，优化这个代价函数来构建视觉-惯性传感器定位和地图构建问题模型：
其中，i是相机序数，k是相机图像帧序数，j是路标序数。第k个帧上可见的路标和第i个相机联立表示为J(i,k)。Wri,j,k表示各个路标观测值的信息矩阵，Wsk表示第k帧IMU误差。
本来，纯视觉SLAM有6个自由度（待确定参数，平移加上旋转），在优化过程中这些参数都需要确定下来。视觉-惯性传感器融合方法只需要4个自由度，因为重力加速度提供2个旋转自由度观测值。这使得取值比较复杂。我们需要确定重力方向（世界坐标中的Z轴）附近的偏向，还有位置，主要是第一个位姿（k1）。因此，除了将位置变化设置为0，即δPwsk1w=03x1，我们还假定δαk1=[δα1k1,δα2k1,0]T。
接下来，我们将推导标准重射影误差等式。然后，将提出一种带有偏移估计的IMU运动模型，这是基于IMU误差函数。
1） 重影射误差等式：我们将论文3中的公式稍加修改，并没有采用标准的等式：
这里，hi(.)表示相机射影模型，Zi,j,k代表图像坐标测量值。误差的雅可比表达式直接采用论文3。
2） IMU运动模型：我们假设地球的自转对陀螺仪的精度影响非常小，就可以把带有简单偏移的IMU的运动模型写为：
这里w:=[wgT,waT,wbgT,wbaT]T的元素是不相关的0均值高斯白噪声（服从高斯概率分布为高斯白噪声）。 是加速度观测值，gw是地球重力加速度向量。与陀螺仪偏移随机游走模型不同，我们采用时间常量τ>0构建加速度传感器随机游走偏移模型。矩阵Ω从角速度的估计值获取 ， 其中陀螺仪的测量值 ：
线性误差动态模型：
其中G向前取值，
（.）×表示矩阵向量的反对称叉积。
注意等式7和等式10的使用方法和经典EKF滤波中均值和方差的迭代方法一致。在实际实现上，这些等式是在离散时间上执行的，序数p表示第p个IMU的测量数据。考虑到计算复杂度，我们采用简单的欧拉前向方法在时间增量⊿t上进行积分。类似的，我们所用的离散时间误差状态变换矩阵为：
协方差迭代计算的等式为：
其中，包含了所有过程的噪声密度 。
3） IMU测量值误差推导
图4表示测量数据输出频率的不同，相机在时刻k和时刻k+1拍摄照片，IMU的测量数据更快一些和相机拍照并不同步。
图4 IMU和相机的不同速率：IMU的加速度传感器和陀螺仪读数在两个连续的相机测量之间输出
我们需要IMU的误差函数 做为机器人状态在k和k+1时刻的一部分，另外还有所有的在这两个时刻间的IMU测量数据（包括加速度计和陀螺仪的读数）统称为 。这里我们必须估计一个机器人在相机拍摄时间k和k+1时刻的状态的正态条件概率密度f：
状态估计 的条件协方差为
IMU误差估计可以记为：
这一基于前一状态的估计与真实状态之间的差别比较简单，除航向外，我们使用了简单的最小误差相乘。
接下来，基于误差迭代法则，相关的系数矩阵 为：
通常，航向误差都是非零的，雅可比矩阵 可以直接计算。
最后， 与 和 相关的雅可比矩阵在优化问题中用于提供一种有效的方法。
与 直接方法不同，需要搞清楚雅可比矩阵的其他部分。IMU误差等式14采用迭代计算。与状态 不同，应用链式规则，生成等式16:
这里，t(.)表示特定步骤的时间戳，pk表示获取相机图像帧k后的第一个IMU采样信号。
C. 关键点匹配和关键帧选择
我们采用SSE优化的Harris角点检测和BRISK描述子特征提取，论文12所示。角点检测要求图像上的关键点规则分布，要求较强角点附近的较弱角点分值均匀变化。描述子的提取沿着梯度方向，梯度变化由于IMU融合可以观测到。
开始的时候，关键点被立体三角化，插入到局部地图中。我们执行暴力匹配所有地图路标；离群点去除采用图像坐标的卡方检验，用IMU积分的位姿估计图像坐标。紧耦合IMU的另一个优势是不需要耗费资源的RANSAC。接下来的优化步骤中，需要维护有限的相机图像序列，比如与某时刻关联的相机位姿；所有这些图像中可见的图标存储在局部地图中。如图5所示，我们讲两类帧分开：包含当前帧的最新帧作为暂时窗口S；过去拍摄的N个关键帧。对于关键帧的选择，云点匹配的图像区域与所有检测到的云点生成的区域的比值低于50到60%时，图像帧别记为关键帧。
D. 部分挑选方法
非线性局部约束存在于一个限定的包含可以随意区分开关键帧的优化窗口，这个约束并不明显。接下来，我们将首先提供一个挑选方法的数学推导，比如消除非线性优化中的状态，将它们用于视觉－惯性传感器SLAM。
1）非线性优化中挑选方法的数学推导：
高斯－牛顿方法从误差项、雅可比矩阵喝系数方程开始构建：Hδx=b。比如一组需要挑选的状态xu，一组与误差项关联的所有状态xλ ， 和一组剩余状态xρ。由于条件独立性，我们可以简化挑选步骤，将其用于一个子问题：
采用舒尔补运算得到：
其中， 是xλ和xu的非线性方程。
等式18描述了挑选中的一个单一步骤。在我们的基于关键帧的方法中，必须能够重复使用挑选这一步骤，将生产的结果作为优化的先验信息，状态估计的值一直都在改变。因此，我们必须修复x0附近的线性点，和任意时刻挑选的x值。增量 是筛选后的状态更新，其中 是x的估计值。 也就是说x由下面的表达式组成：
有了这个一般化的表达式，我们就可以用最小化坐标中的先验信息作用于状态变量，包括单位长度四位数。引入Δx右边的式子可以近似为：
那么式（17）高斯-牛顿方法就可以表示为：
在这种等式形式下，等式（18）右边就等于：
在这里，无穷远处（或接近无穷远处）挑选的节点组成路标，或者路标只在相机的某个位姿可见，与那些路标关联的海塞矩阵块往往是等级比较低的。因此，我们采用伪倒置矩阵 ，
它提供了一种零空间给定δxλ求δxμ的方法。
上述等式给挑选的状态xμ和剩余状态xλ引入了一个固定线性点。这给后续与这些状态相关线性化提供了一个参考点。使用等式（18），我们可以去掉非线性部分，将挑选的 作为被加数构建整体高斯-牛顿系统。卡方分布误差可以写为： 。
2）挑选方法应用于基于关键帧的视觉-惯性传感器SLAM
初始挑选误差可以从前N+1帧中构建，它们都带有速度和偏移，如图6所示。每个N的第一帧都被认为是关键帧，挑选步骤是去除对应的速度和偏移。
图6. 图表示在前N+1帧上初始化挑选方法
新的帧 （当前帧，序号c） 插入到优化窗口时， 就运行筛选操作。这时，时间窗口中最老的帧就不是关键帧，我们就丢掉所有的路标观测值，将他们和最老的速度和偏移状态一起挑选出来。图7演示了这个过程。丢掉路标观测值是局部最优的；然而，对快速解决方案它使问题稀疏化。基于关键帧的视觉SLAM处理过程也是类似的，会丢掉整个帧和路标测量值。
图7. N=3个关键帧和IMU/时刻节点S＝3。时间窗口中滑出一个常规帧。
在这个例子中， 作为关键帧，丢掉所有关键帧观测值，信息丢失就更明显：所有具有共同路标观测值的两个最老的关键帧之间的位姿信息将会丢失。 因此，我们要剔除那些在 可见，但在大多数最近的关键帧中不可见的路标。 图8显示了这个过程。再次使用了这个问题的稀疏性。
三 结论
我们用图1中定制的传感器模块原型做了一些实验，提供WVGA立体图像，14厘米基准线与IMU（ADIS16488）同步。这是实时的实验，运行在电脑上（2.2 GHz Quad-Core Intel Core i7, 8 Gb RAM）。我们用g2o作为优化框架。比较精确的与IMU相关的相机内参和外参之前已经有了，如论文4所示。IMU参数如表1所示。
我们采用论文5的方法：对多次启动，基准和估计轨迹需要对其，误差用于估计偏移距离。紧耦合算法评估不需要基准，而视觉和松耦合方法需要。为了对比估计算法，我们将所有算法的特征匹配和紧耦合的固定。视觉方法的估计作为论文20松耦合基准算法的输入（固定尺度和内部传感器标定）。
A. Vicon：绕着圈子走
即使在房间内绕圈行走，视觉-惯性传感器配备了Victor相机提供了200Hz下精确的6D位姿。不需要回环闭合算法，实验运动是90米。图9演示了时序上的位置和航向误差。对重力方向而言，对飞行系统非常重要，松耦合方法可以控制航向误差。紧耦合方法的误差最小，与位置关系非常明显。
图9. 与Vicon基准的对比。所有情况的关键帧测量。第5个和第95个百分数，还有10米内的平均值。
B. 汽车：室外长距离轨迹
传感器在汽车顶部，同步捕捉6D GPS-INS基准工作频率100Hz，轨迹8公里。图10 是带有基准的估计轨迹顶视图对比。图11是平移和旋转误差的定量对比，反映了视觉和惯性传感器观测值紧融合状态下显著增强。与预计的情况一样，松耦合方法和视觉方法性能相似。主要因为松耦合方法在长时间水平运动状态下而不是在重力方向上，没有增强位姿估计。
C. 建筑：长期室内回环
最后一个实验是手持传感器长时间作室内回环上5层楼。没有基准，我们只能做定性评估，用我们算好的方法做室内3D重构，叠加视觉轨迹作对比。这显示了走廊和楼梯上光照和纹理条件下的挑战。图12的顶视图演示了回环误差0.6米和视觉误差2.2米的对比。
四 结论
本文演示了基于关键帧视觉SLAM集成惯性传感器的紧耦合方法。非线性优化中的误差估计包括关键点检测和IMU数据读取，优于参数调整。使用这些方法，可以获得重力方向全局一致性和采用IMU运动模型的鲁棒的离群点去除方法。同时，也使用了关键帧非线性优化，比如位姿不动时。采用实时立体相机和IMU融合，精度和鲁棒性优于视觉或松耦合方法。
参考文献
[1] T. Barfoot, J. R. Forbes, and P. T. Furgale. Poseestimation using linearized rotations and quaternionalgebra. Acta Astronautica, 68(12):101 – 112, 2011.
[2] T-C. Dong-Si and A. I. Mourikis. Motion trackingwith fixed-lag smoothing: Algorithm and consistencyanalysis. In Proceedings of the IEEE InternationalConference on Robotics and Automation (ICRA), 2011.
[3] P. T. Furgale. Extensions to the Visual OdometryPipeline for the Exploration of Planetary Surfaces.PhD thesis, University of Toronto, 2011.
[4] P. T. Furgale, J. Rehder, and R. Siegwart. Unified temporaland spatial calibration for multi-sensor systems. In Proc. of the IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), 2013. Toappear.
[5] A. Geiger, P. Lenz, and R. Urtasun. Are we readyfor autonomous driving? the KITTI vision benchmark suite. In Proc. of the IEEE Conference on ComputerVision and Pattern Recognition (CVPR), 2012.
[6] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I.Roumeliotis. Towards consistent vision-aided inertialnavigation. In Proc. of the Int’l Workshop on theAlgorithmic Foundations of Robotics (WAFR), 2012.
[7] V. Indelman, S. Williams, M. Kaess, and F. Dellaert.Factor graph based incremental smoothing in inertial navigation systems. In Information Fusion (FUSION),International Conference on, 2012.
[8] E. S. Jones and S. Soatto. Visual-inertial navigation,mapping and localization: A scalable real-time causalapproach. International Journal of Robotics Research(IJRR), 30(4):407–430, 2011.
[9] J. Kelly and G. S. Sukhatme. Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor selfcalibration.International Journal of Robotics Research(IJRR), 30(1):56–79, 2011.
[10] K. Konolige, M. Agrawal, and J. Sola. Large-scale visualodometry for rough terrain. In Robotics Research,pages 201–212. Springer, 2011.
[11] R. K¨ummerle, G. Grisetti, H. Strasdat, K. Konolige,and W. Burgard. g2o: A general framework for graph optimization. In Proceedings of the IEEE InternationalConference on Robotics and Automation (ICRA), 2011.
[12] S. Leutenegger, M. Chli, and R.Y. Siegwart. BRISK:Binary robust invariant scalable keypoints. In Proceedingsof the IEEE International Conference onComputer Vision (ICCV), 2011.
[13] C. Mei, G. Sibley, M. Cummins, P. M. Newman, andI. D. Reid. Rslam: A system for large-scale mappingin constant-time using stereo. International Journal ofComputer Vision, pages 198–214, 2011.
[14] A. I. Mourikis and S. I. Roumeliotis. A multistateconstraint Kalman filter for vision-aided inertialnavigation. In Proceedings of the IEEE InternationalConference on Robotics and Automation (ICRA), 2007.
[15] A. Ranganathan, M. Kaess, and F. Dellaert. Fast 3dpose estimation with out-of-sequence measurements.In Proc. of the IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2007.
[16] G. Sibley, L. Matthies, and G. Sukhatme. Slidingwindow filter with application to planetary landing.Journal of Field Robotics, 27(5):587–608, 2010.
[17] D. Sterlow and S. Singh. Motion estimation from imageand intertial measurements. International Journalof Robotics Research (IJRR), 23(12):1157–1195, 2004.
[18] H. Strasdat, J. M. M. Montiel, and A. J. Davison. Realtimemonocular SLAM: Why filter? In Proceedingsof the IEEE International Conference on Robotics andAutomation (ICRA), 2010.
[19] B. Triggs, P. Mclauchlan, R. Hartley, and A. Fitzgibbon.Bundle adjustment – a modern synthesis. InVision Algorithms: Theory and Practice, September,1999, pages 298–372. Springer-Verlag, 1999.
[20] S. Weiss, M.W. Achtelik, S. Lynen, M. Chli, andR. Siegwart. Real-time onboard visual-inertial stateestimation and self-calibration of MAVs in unknownenvironments. In Proc. of the IEEE InternationalConference on Robotics and Automation (ICRA), 2012.
