视觉SLAM理论与实践进阶-手写VIO_1

一、VIO文献综述

1.1 视觉与IMU进行融合之后有何优势？
1). 纯视觉 SLAM 容易受图像遮挡,光照变化,运动物体干扰,弱纹理场景等影响,单目视觉无法测量尺度,深度信息往往无法直接获取,仅通过单张图像无法获得像素的深度信息,需要通过三角测量(Triangulation)的方法来估计地图点的深度。此外单目纯旋转运动无法估计,快速运动时易丢失。

通过本人之前跑通的视觉 SLAM 框架 ORB-SLAM2(前端采用 ORB 特征提取算法)的实验结果可以侧面证实以上特点,如在运行 ORB-SLAM2 时白墙往往无法提取特征点,在实验室走廊实时运行时,尝试这把摄像头摆到只有白墙的方向,发现特帧点提取不到,另一个窗口中地图轨迹跟丢。包括在做室外实验时,从室内走到室外的过程,特侦点也容易丢失,可能是光照变化导致的。快速运动同样出现这种情况,但是特帧点丢失后将摄像头转向之前走过的场景,特帧点又会重新获取。

2). IMU 可以获取加速度和角速度信息,而且输出频率较高(200HZ 左右,摄像头一般20HZ 左右),但是 IMU 获取的测量量由于是通过积分运算得到的,不可避免的存在误差累积。对于短时间的位姿测量效果较好,但是长时间就会存在较大误差。(当然根据 IMU 自身精度而定,价格越高误差累积越小,这里几十上百万的 IMU 不在讨论范围之内,根据课题需求考虑的是低成本传感器)。IMU 由于是自主导航系统,它不容易受外界环境影响(据了解温度对其有一定的影响),如在弱 GPS 信号区域,在夜晚光线较暗区域等,如果没有激光雷达,视觉传感器又受影响则 IMU 可以提供较好的定位信息。

综上所述将两者通过某种算法或结构融合起来形成优势的互补,取长补短,这一点在之前的无人驾驶方案中已有前例,如 GNSS/IMU 通过卡尔曼滤波器融合,诺瓦泰已经有相关的产品上市,如一体式高精度 MEMS 组合导航系统 SPAN-CPT7(50 万左右),高精度闭环光纤组合导航系统 SPAN-ISA-100C(100 万左右)。搭配后处理软件可实现无人驾驶厘米级精度,但是价格相当昂贵,作为参考系统还行,不可能每辆车都搭配这么贵的传感器。 imu加摄像头的组合能实现高精度的同时,价格成本也不高。

1.2 有哪些常见的视觉+IMU 融合方案?有没有工业界应用的例子?学术界研究进展如何?
1).视觉+IMU 融合方案

VIO框架	耦合方案	后端方案	前段	视觉误差	初始化	回环	精度	效率
MSF	松耦合	EKF	—	—	—	—	最差	最快
MSCKF	紧耦合	EKF	Fast+光流	重投影	静止	无	较差	较快
ROVIO	紧耦合	IEKF	Fast+光度	光度	静止	无	一般	较快
OKVIS	紧耦合	优化	Harris+BRISK	重投影	无	静止	较好	最慢
VINS	紧耦合	优化	Harris+光流	重投影	动态	有	最好	较慢
VIORB	紧耦合	优化	Orb	重投影	动态	有	—	—
ICE-BA	紧耦合	优化	Fast+光流	重投影	静止	无	—	—

以上基本开源,其中 VI-ORB 官方并未开源,由清华大学王京复现,目前王京版本能在在网上找到。
2)工业界应用
Google: Tango(目前已停产),ARCore
Apple: ARKit
Microsoft: HoloLens
Baidu: DuMix AR
3)学术界研究进展
目前学术界(就我目前看到的相关论文而言),针对每个开源框架的特点(上表所示),应用于特定场景下加以改进。上海交大邹丹平教授 2019 年发表在 IEEE TANSACTIONS ON ROBOTICS 的一篇文章”StructVIO: Visual-Inertial Odometry With Structural Regularity of Man-Made Environments” [1] .其中针对外部场景的结构线作为前端特征进行提取,后端采用 EKF 滤波方案。通过在 Euroc 数据集的几个序列中进行实验对比了 OKVIS/VINS 发现 StructVIO 的精度最好。

像 PL-SLAM [2] ,PL-VIO [3] 等研究都是将线段跟踪加入到前端,作为特征点的补充,使得系统鲁棒性更高。
前端的好坏对整个slam系统性能的影响给人的感受最为直观。佐治亚理工学院的赵亦璞 2019 年在网上(arXiv)发布的一篇文章”Good Feature Selection for Least Squares Pose Optimization in VO/VSLAM” [4] 。也是对前端进行了改进,这篇文章提出了一种特征选择的算法,从而提高了整个系统的精度,据了解这种方法可以对目前主流的框架进行改进,详细的可以参考
github链接
基于深度学习的SLAM,最近也有研究人员将深度学习的方法引入到SLAM中，具体分为一下三块：
1)端到端(End To End)的SLAM
2)语义SLAM
3)用深度学习方法替换传统SLAM中的一个或几个模块
综上所述,类似的文章在网上较多,大多都是基于前端的改进（也可能是本人目前的关注点在前端，该描述不一定准确，希望读者选择性吸收）,目前后端提出了什么新的优化或滤波算法投入应用还有待了解。后续更新。。。

二、四元数和李代数更新

用四元素和旋转矩阵的方式分别对旋转进行更新：

$$R\leftarrow Rexp\left({\omega }^{\wedge }\right)$$
$$q\leftarrow q\otimes \left[1,\frac{1}{2}\omega \right]$$
请用以上两种方法编程验证对于如下小量的结果
$$\omega ={\left[0.01,0.02,0.03\right]}^{\top }$$
此处贴出部分代码如下，完整运行代码点击这里下载

//李代数更新
Eigen::Vector3d update_so3(0.01,0.02,0.03); //更新量
Sophus::SO3 SO3_updated=SO3_R*Sophus::SO3::exp(update_so3); //右乘更新
cout<<"SO3 updated="<<endl<<SO3_updated.matrix()<<endl;

//四元素更新
Eigen::Quaterniond q_update(1,update_so3(0)/2,update_so3(1)/2,update_so3(2)/2);
Eigen::Quaterniond q_updated=(q*q_update).normalized(); //四元素更新并归一化
cout<<"q2R="<<endl<<q_updated.toRotationMatrix()<<endl;

运行运行结果截图如下：
显然可以看出两种方式更新结果非常相近。如果读者感兴趣可以用罗德里格斯公式进行旋转更新试一试，结果也是一样的。。。

三、 导数公式的推导

使用右乘 so(3),推导以下导数:

$$\frac{\mathrm{d}({\mathrm{R}}^{-1})\mathrm{p}}{\mathrm{d}R}$$

$$\frac{\mathrm{d}\mathrm{l}\mathrm{n}({\mathrm{R}}_1{\mathrm{R}}_2^{-1}{)}^{\vee }}{\mathrm{d}{R}_2}$$
证明直接上图：
[1]Zou D , Wu Y , Pei L , et al. StructVIO : Visual-inertial Odometry with Structural Regularity of
Man-made Environments[J]. 2018.
[2]Pumarola A, Vakhitov A, Agudo A, et al. PL-SLAM: Real-time monocular visual SLAM with
points and lines[C]// IEEE International Conference on Robotics & Automation. 2017.
[3] Yijia H , Ji Z , Yue G , et al. PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using
Point and Line Features[J]. Sensors, 2018, 18(4):1159-.
[4]Zhao Y , Vela P A . Good Feature Selection for Least Squares Pose Optimization in VO/VSLAM[J].
2019.

友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！