视觉惯性SLAM: VI ORB-SLAM

  • 这篇博客
  • 视觉惯性SLAM
  • 预备知识
    • 符号说明：
    • 相机投影变换矩阵
    • IMU数据更新方程
    • IMU数据的预积分
  • VI ORB-SLAM各环节工作方式
    • Initialization
    • Tracking
    • LocalMapping
    • Loop Closing
    • Full BA
  • IMU初始化
    • 估计bgb_{g}bg​
    • 估计尺度sss和重力向量gWg_{W}gW​（不考虑加速度偏差bab_{a}ba​）
    • 估计加速度偏差(bab_{a}ba​)，尺度(sss)和重力方向
    • 速度(vvv)估计
    • 在重定位后重新初始化bg,bab_{g},b_{a}bg​,ba​
  • 结尾

这篇博客

 之前看了VI ORB-SLAM的论文，现在终于有时间来写论文的学习笔记(不容易啊)。因为这是第一次看视觉惯性SLAM论文，所以写这篇博客来详细地记录一下，以后发现问题会再做修改。话不多说，直接进入正题。
【转载声明】本篇文字均为原创，如转载请注明出处

视觉惯性SLAM

 只使用相机或惯性元件(IMU)传感器搭建的SLAM系统会存在诸多问题。因为相机虽然可以很好地完成闭环检测和定位，但是它易受到模糊等影响，所以运动速度不能过快。而IMU却可以在速度较快，或周围环境纹理较少时完成定位操作。不过它不能实现闭环检测，且很容易产生漂移现象（因为存在加速度偏差和陀螺仪偏差：即使IMU元件不动，测量值也可能不为0）。
 这么看来这两个传感器的功能刚好能够互补，用相机来帮助IMU消除累积误差，并完成闭环检测；用IMU帮助相机解决在纹理少、运动快的情况下的定位问题。因此，视觉惯性SLAM也就诞生了。
 VI ORB-SLAM是由ORB-SLAM和IMU模块组成，它的大部分环节功能和ORB-SLAM是相似的，所以相似部分之后就不会过多地介绍。

预备知识

符号说明：

 这里提前说一些之后会在公式中使用的符号和变量。
1、惯性传感器（IMU）的坐标系记为：BBB (Body)
2、相机传感器的坐标系记为：CCC（Camera）
3、世界坐标系记为：WWW
4、IMU中的加速度偏差：bab_{a}ba​
5、IMU中的陀螺仪偏差：bgb_{g}bg​
6、IMU中两次测量值之间的时间间隔：Δt
7、IMU在世界坐标系中的方向：RWBR_{WB}RWB​（从B坐标系到W坐标系的旋转矩阵）
8、将B坐标系的速度在W坐标系中表示：WVB_{W}V_{B}W​VB​
9、将B坐标系的位置在W坐标系中表示：WPB_{W}P_{B}W​PB​（B到W坐标系之间的平移变换）
10、重力加速度：gWg_{W}gW​
11、重力方向：gW^=gW∗∥gW∗∥\hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|}gW​^​=∥gW∗​∥gW∗​​
12、重力的大小GGG
13、IMU测得的加速度大小：aBa_{B}aB​
14、IMU测得的角速度大小：wBw_{B}wB​
15、B坐标系到相机坐标系的变换矩阵：TCB=[RCB∣CPB]T_{CB}=[R_{CB} | _{C}P_{B}]TCB​=[RCB​∣C​PB​]

相机投影变换矩阵

 基于小孔成像模型来算出相机坐标系与像素坐标系之间的变换矩阵，并定义投影函数π\piπ:
  π(XC)=[fuXCZC+cufvYCZC+cv],XC=[XCYCZC]T.............(1)\pi(X_{C})=\begin{bmatrix} f_{u}\frac{X_{C}}{Z_{C}}+c_{u}\\ f_{v}\frac{Y_{C}}{Z_{C}}+c_{v} \end{bmatrix} ,X_{C}=[ X_{C} Y_{C} Z_{C}]^{T}.............(1)π(XC​)=[fu​ZC​XC​​+cu​fv​ZC​YC​​+cv​​],XC​=[XC​YC​ZC​]T.............(1)
式中 [fu,fv]T[f_{u},f_{v}]^{T}[fu​,fv​]T表示焦距长度，[cu,cv][c_{u},c_{v}][cu​,cv​]表示主点位置。它们的单位都为像素。

IMU数据更新方程

 更新的量有三个：R，p，vR，p，vR，p，v。更新的公式如下：

 这个公式是从参考文献[1]中获得的，表示的是前后两帧之间的变量关系（和参考文献[1]不同的是，这里忽略了噪声对角速度、加速度测量值的影响）。这是之后实现IMU数据预积分的基础。

IMU数据的预积分

 预积分就是计算出两相邻帧或关键帧之间的IMU数据的总变化量（ΔR，Δp，Δv）（ΔR，Δp，Δv）（ΔR，Δp，Δv），它由相邻帧或关键帧之间所有的IMU测量数据叠加后获得。在VI ORB-SLAM中用这个公式表示预积分：

 这个公式也是从参考文献[1]中获得的。其中JΔvgbgiJ_{\Delta v}^{g}b_{g}^{i}JΔvg​bgi​表示第 i 帧的陀螺仪偏差对于ΔvΔvΔv的影响量，JΔvgJ_{\Delta v}^{g}JΔvg​是ΔvΔvΔv关于bgb_{g}bg​的雅可比矩阵。其他与此类似的变量也是这个意思。

VI ORB-SLAM各环节工作方式

 因为是基于ORB-SLAM搭建的系统，所以VI ORB-SLAM的工作原理和ORB-SLAM十分相似，也是使用三个线程：Tracking、Local Mapping、Loop Close。不过它们各线程的具体工作内容有所不同，下面来介绍这些不同之处。

Initialization

 在ORB-SLAM中，初始化的任务有：创建出初始地图、定义第一参考帧和轨迹的尺度。在VI ORB-SLAM中，除了上述任务外，初始化还要估计出IMU设备的重力方向、加速度和陀螺仪偏差等变量。它的步骤是：
1、先使用最原始的视觉初始化，获得初始地图和一定数量的关键帧；
2、进行IMU传感器的初始化，初始化其重力方向、加速度和陀螺仪偏差、速度，以及整个轨迹对应的尺度。
 这个部分的内容放在最后再说。

Tracking

 跟踪线程负责计算状态变量：(R,p,v,b)(R,p,v,b)(R,p,v,b)，即机器位姿、速度和IMU传感器的偏差。计算速率和相机的帧率相同（因为IMU数据的采集速率快，不能给每个数据找到其对应的图像数据。图像数据则刚好相反）。
 在估计出状态变量后，对估计值进行优化。所以整个线程的工作流程如下：
1、估计出当前帧的状态变量；
2、将地图中的地图点投影到当前帧上，寻找匹配特征点；
3、计算这些匹配点的重投影误差以及相应帧之间的IMU误差；
4、求得总误差，通过最小化总误差来完成状态变量的优化。

图1

 这里需要注意的是：根据地图是否更新(有无新的关键帧产生或检测到闭环)，上述流程中第3、4步会有两种不同的计算方法
(a)当地图发生了更新：只有优化当前帧的状态
 使用更新后的地图和最后一个关键帧作为优化的参考，它们在优化过程中保持不变。如下图所示：

图2

图中来自最后一个关键帧的变量用下标 iii 表示，来自当前帧的则用下标 jjj 表示。重投影误差用蓝色矩形表示，IMU误差则是绿色矩形。红色框表示固定的变量。
 在当前情况下，待优化的状态变量和优化方式为：

式中EprojE_{proj}Eproj​表示地图点与当前帧产生的重投影误差，EIMUE_{IMU}EIMU​表示关键帧与当前帧产生的IMU误差。EprojE_{proj}Eproj​的计算公式:

式中XCX_{C}XC​是地图点在相机坐标系下的坐标(计算原理可理解为：WpB_{W}p_{B}W​pB​是IMU在世界坐标系下的坐标，用XWX_{W}XW​（地图点的世界坐标）减去WpB_{W}p_{B}W​pB​就能获得该地图点与WpB_{W}p_{B}W​pB​之间的向量(世界坐标系)。然后对该向量进行旋转平移变换，就能获得地图点在B坐标系下的坐标)。
 IMU误差的计算公式为：

 式6是在文献[1]中获得的。
个人理解：感觉ev,epe_{v}, e_{p}ev​,ep​的计算和文献[1]中不符，使用的b(.)jb_{(.)}^{j}b(.)j​应该是b(.)ib_{(.)}^{i}b(.)i​。
 之后使用非线性优化的方式优化误差。最后将当前帧的优化结果和Hessian矩阵设为一个先验信息，给后续帧做参考。

(b) 地图未更新：同时优化前后两帧，但保持地图点不变

图3

 从图中可看出，这个情况下只用地图点保持不变，且优化的对象从一个当前帧增加到前后两帧，对应的待优化状态变量也变成了：

式中EpriorE_{prior}Eprior​是前一帧保留的先验信息产生的先验误差（图3中的小灰色正方形），它的计算公式为：

式中带横线的变量表示前一帧的先验信息，不带横线的表示前一帧在当前优化中的结果。ρρρ是一类鲁棒核函数。
个人理解：EpriorE_{prior}Eprior​：在当前优化过程中可以调节前一帧的状态变量，但不能使其变化太多(好像可以防止累积误差)
 在完成优化后，上一帧就被边缘化（抛弃），而当前帧的优化结果又将作为新的先验信息。这个方法一直持续到地图发生更新或先验信息无效为止。

LocalMapping

 VIORB-SLAM和ORB-SLAM的局部建图环节不同的地方在于：前者在进行LocalBA时要多考虑一个IMU的误差项约束。这两个系统在LocalBA上的对比图如下：

图4

相同点：只保留和优化N个关键帧，以及它们观测到的所有地图点。不过其余能够观测到这些地图点的关键帧也会在LocalBA中被使用。它们给地图点提供重投影误差的约束，自身的状态是不变的（在红色框内的部分）。
不同点：VIORB-SLAM要考虑各关键帧之间的IMU约束，以及第N+1个关键帧（红框最右边）对其下一关键帧的IMU约束。

Loop Closing

 VIORB-SLAM 和 ORB-SLAM在闭环检测环节上是差不多的。当检测到闭环之后，会对闭环中的关键帧进行位姿图优化（减少计算量），只优化位姿（R,p）（R,p）（R,p），而不优化IMU变量（v，b）（v，b）（v，b）。
 值得注意的是，位姿图中优化的状态变量只有6个自由度，因为尺度信息可以在IMU初始化时估计出来。这和ORB-SLAM是不同的。
个人理解：因为闭环匹配上的两个关键帧之间很难有IMU的数据约束，所以位姿优化不优化IMU变量

Full BA

 使用一个独立的线程进行Full BA，优化所有状态量：位姿，速度和偏差。

IMU初始化

 这是整个VI ORB-SLAM的核心部分，用于估计重力方向、尺度(sss)、速度(vvv)、IMU的偏差(ba,bgb_{a},b_{g}ba​,bg​)。整个过程为：
1、估计陀螺仪偏差(bgb_{g}bg​)
2、粗略估计尺度(sss)和重力向量(gWg_{W}gW​)（不考虑加速度偏差的影响）
3、估计加速度偏差(bab_{a}ba​)，并优化尺度(sss)和重力方向
4、估计速度(vvv)
需要注意的是任两个相邻的关键帧的时间间隔要短，以减少测量噪声的影响。下面介绍各步骤的具体内容。

估计bgb_{g}bg​

 可以通过每两个相邻关键帧之间的旋转测量值来估计陀螺仪偏差：

式中$R_{(.)}$表示由视觉求出的关键帧间的旋转变换，$\Delta R_{i,i+1}$表示通过预积分求出的关键帧间变换关系。

 优化过程中假设bgb_{g}bg​保持不变，通过最小化上述误差项来估计出bgb_{g}bg​。

估计尺度sss和重力向量gWg_{W}gW​（不考虑加速度偏差bab_{a}ba​）

 在求出bgb_{g}bg​之后，可以预积分出速度(vvv)和位置(ppp)。
 由于单目SLAM估计的轨迹存在一个尺度因子sss，所以在轨迹中描述IMU元件与相机之间的变换关系时要考虑sss，即构建如下变换公式：

 将式(10)带入到式(3)，同时忽视其中的bab_{a}ba​后可得公式：

 该式子用于求解 sss 和 gWg_{W}gW​ ，由求解线性方程来实现。为了避免求解速度变量，降低方程复杂度，使用三个连续关键帧之间存在的两个相对位姿关系，以及式子(3)中的速度计算公式，来构建线性方程组：

 为了方便，将关键帧 i、i+1、i+2i、i+1、i+2i、i+1、i+2写作1、2、31、2、31、2、3。方程组中各项的表达式为：

 方程组(12)的形式为A3(N−2)X4x4X1=B3(N−2)X1A_{3(N-2)X4}x_{4X1}=B_{3(N-2)X1}A3(N−2)X4​x4X1​=B3(N−2)X1​。使用SVD求解方程组(12)，获得s∗,gW∗s^{*}, g_{W}^{*}s∗,gW∗​。由于未知量的自由度为 4，所以至少需要有4个关键帧才能求解方程组。

估计加速度偏差(bab_{a}ba​)，尺度(sss)和重力方向

 前面获得的s∗,gW∗s^{*}, g_{W}^{*}s∗,gW∗​未考虑加速度偏差(bab_{a}ba​)，原因是加速度偏差和重力很难区分开==(重力会影响到bab_{a}ba​，两者相关性较大)==。而重力的方向和加速度偏差区分性很好，所以可以同时估计这两者的量。
 首先认为在地图的第一参考惯性坐标系I中，重力的方向为gI^={0,0,−1}\hat{g_{I}}=\{0,0,-1\}gI​^​={0,0,−1}，大小为GGG。而之前计算出来的gW∗g_{W}^{*}gW∗​的方向为gW^=gW∗∥gW∗∥\hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|}gW​^​=∥gW∗​∥gW∗​​。这时计算W、IW、IW、I坐标系之间的旋转变换：

个人理解：之前求出的gW∗g_{W}^{*}gW∗​是在地图第一参考帧中的重力在世界坐标系下的向量表示，而gIg_{I}gI​是重力方向在第一参考帧中的表示。求出RWIR_{WI}RWI​后就能够获得所有轨迹与真实世界之间的旋转关系(修正位姿估计值的方向)
 此时gWg_{W}gW​可以表示成：gW=RWIgI^Gg_{W}=R_{WI}\hat{g_{I}}GgW​=RWI​gI​^​G …(15)

式中的RWIR_{WI}RWI​只包含x、y两个轴上的旋转角度，因为z轴的旋转不会影响到重力的方向。
 通过下面这个扰动来优化旋转RWIR_{WI}RWI​：

 式(16)的一阶近似为：

(PS：关于扰动和一阶近似可以参考《视觉SLAM十四讲》P75、P110)

 将式(17)带入到式(11)中，并考虑bab_{a}ba​的影响，可得：

 仍是使用三个关键帧，得到如下线性方程组：

其中各项的表达式为($\lambda (i)$和方程组(12)中的一样)：

式中[](:,1:2)[]_{(:,1:2)}[](:,1:2)​表示矩阵的前两列
 方程组(19)的形式为A3(N−2)X6x6X1=B3(N−2)X1A_{3(N-2)X6}x_{6X1}=B_{3(N-2)X1}A3(N−2)X6​x6X1​=B3(N−2)X1​。通过SVD的方法求解(19)，求出s∗,δθxy∗,ba∗s^{*},\delta \theta _{xy}^{*} , b_{a}^{*}s∗,δθxy∗​,ba∗​，以及条件数。由于待求变量的自由度为6，所以至少需要4个关键帧。
PS：条件数可用于检查此问题是否条件良好(如：IMU执行的运动使所有变量均可观察)[2]

速度(vvv)估计

 可以通过式(18)求出（用来初始化使用的）关键帧的速度，因为此时s、gW、ba、bgs、g_{W}、b _{a}、b_{g}s、gW​、ba​、bg​都已知。此外，还可以使用公式(3)求出最近的关键帧的速度。

在重定位后重新初始化bg,bab_{g},b_{a}bg​,ba​

 如果系统在长期运行后进行了重定位操作，则使用式(9)重新初始化bgb_{g}bg​。而bab_{a}ba​则通过式(19)求出，此时s、gWs、g_{W}s、gW​都是已知的。此过程使用20个连续的帧来完成，这些帧都是由视觉估计位姿。

结尾

 这篇博客是自己对视觉惯性SLAM领域的一个初步探索，里面也加入了一些自己的理解。虽然VI ORB-SLAM的作者没有开源代码，但是网上有前辈根据论文复现了系统(感谢前辈！！)。所以又有事情可做了/(-o-)/~~。

参考资料：
1、https://arxiv.org/pdf/1610.05949.pdf
2、https://blog.csdn.net/myarrow/article/details/54694472(这篇博客写的真不错！！)

视觉惯性SLAM: VI ORB-SLAM相关推荐

  1. 【ORB-SLAM3论文翻译】ORB-SLAM3：一个精确的视觉、视觉惯性和多地图SLAM的开源库（注：带着原文看，很多公式和变量不好输入）

    文章目录 前言(非论文部分) 摘要 1. 简介 2. 相关工作 3. 系统概述 4. 相机模型 5. 视觉惯性SLAM(重点) 6. 地图合并与闭环 7. 实验结果 8. 结论 REFERENCES( ...

  2. ORB-SLAM3:一个高精度视觉、视觉惯性和多地图SLAM开源库

    ORB-SLAM3:一个高精度视觉.视觉惯性和多地图SLAM开源库(论文翻译) Carlos Campos, Richard Elvira∗, Juan J. G´omez Rodr´ıguez, J ...

  3. 三维视觉惯性SLAM的有效Schmidt-EKF

    三维视觉惯性SLAM的有效Schmidt-EKF An Efficient Schmidt-EKF for 3D Visual-Inertial SLAM 论文地址: http://openaccess ...

  4. 视觉惯性单目SLAM （二） 算法基础知识

    1. 基本概念 视觉惯性:Visual-Inertial (VI) VI ORB-SLAM:视觉惯性ORB-SLAM VI ORB-SLAM输入: IMU数据(用B表示,加速度:aB:角速度:ωB\m ...

  5. 视觉惯性单目SLAM （一）算法描述

    1.视觉惯性ORB-SLAM基本知识 视觉惯性:Visual-Inertial (VI) VI ORB-SLAM:视觉惯性ORB-SLAM VI ORB-SLAM输入: IMU数据(用B表示,加速度: ...

  6. ICRA 2021论文汇总：视觉-惯性/视觉SLAM

    作者丨zzyy@知乎 来源丨https://zhuanlan.zhihu.com/p/388715878 编辑丨3D视觉工坊 一.视觉惯性组合(21) 1.cooperative Transporta ...

  7. 视觉惯性SLAM：VINS-Mono

    视觉惯性SLAM:VINS-Mono 这篇博客 一些符号说明 IV 测量数据的预处理 A.视觉处理前端 B.IMU预积分 V. 初始化 A.Vision-Only SfM in Sliding Win ...

  8. 一种视觉惯性+激光传感器的SLAM系统

    一种视觉惯性+激光传感器的SLAM系统 这篇博客 论文摘要 一些假设和标注 系统总览 VI 里程计 扫描匹配(scan matching)优化 提高系统鲁棒性的措施 闭环检测和临近检测 全局位姿图优化 ...

  9. 【开源分享】VIDO-SLAM：一种视觉惯性动态物体SLAM系统

    文章:Inertial-Only Optimization for Visual-Inertial Initialization 作者:Carlos Campos, Jose M.M. Montiel ...

最新文章

  1. flask-admin 快速打造博客 系列一
  2. 读《分布式一致性原理》JAVA客户端API操作3
  3. unity3d 随机生成地形之随机山脉
  4. Top10 Web2.0在线生成器
  5. P3175-[HAOI2015]按位或【min-max容斥,FWT】
  6. spring java配置_Spring：使基于Java的配置更加优雅
  7. div独占一行 html_web前端基础-HTML及CSS选择器
  8. 生成静态页面的五种方案 收藏
  9. cpu计算速度排行榜_CPU速度的计算方法和单位
  10. 修改APACHE的默认站点
  11. linux的虚拟内存是4G，而每个进程都有自己独立的4G内存空间，怎么理解？
  12. Win11未安装Defender怎么办？解决Win11未安装Defender的方法
  13. Lua 函数参数 默认实参
  14. 单片机牛人的学习经历
  15. tomcat打印日志乱码_针对tomcat日志乱码问题
  16. http://localhost:8080/login的密码和账号的问题
  17. android hdmi拔插广播,Android drivers/switch驱动详解（用于通过GPIO状态检测耳机、HDMI等的插拔状态）...
  18. [Python]TempConvert.py(温度转换)解释拓展
  19. 简单的RTSP播放器
  20. NAND_FLASH_内存详解与读写寻址方式

热门文章

  1. Python 标准库之 fcntl
  2. 并发 IO多路复用 select 非asyncio
  3. LeetCode简单题之反转单词前缀
  4. QNX Hypervisor管理程序
  5. 传感器融合带来多重好处
  6. 编译器设计-RunTime运行时环境
  7. 2021年大数据Flink（三十五）：​​​​​​​Table与SQL ​​​​​​案例二
  8. DNC-cs6200 ospfv3
  9. 快速android app开发,快速學會開發 Android App
  10. adb.exe: more than one device/emulator