点击上方“计算机视觉life”，选择“星标”
快速获得最新干货
作者丨Alvin一路向东@知乎
来源丨https://zhuanlan.zhihu.com/p/463778905
编辑丨计算机视觉life
LVI-SAM是MIT团队的TixiaoShan等人开源的一个紧耦合的激光雷达视觉惯导SLAM系统，可以实时高精度鲁棒的进行状态估计和建图。哈工大博士带你学习，1个月的学习挑战赛，详情点击 MIT团队开源的这个激光雷达视觉惯导SLAM系统，太香了！

前言

本文是LVI-SAM学习系列的第三部分，在深入分析LVI-SAM源码之前阅读论文原文，可以在分析源码遇到困难时明确思路，少走弯路，提高源码分析效率。
本文主要就是对论文原文精华部分的翻译，主要面向SLAM小白，大神不必在此浪费时间。如有错误，欢迎指出。

一、摘要

LVI-SAM是一种基于平滑化和映射实现的激光雷达-视觉-IMU紧耦合SLAM方法，能够实现高精度、高鲁棒性地实时状态估计和地图构建。
LVI-SAM建立于包含两个子系统的因子图上：视觉-IMU子系统（VIS）和激光-IMU子系统（LIS），两个子系统采用紧耦合设计方式。其中VIS利用LIS进行初始化，利用LiDAR测量的深度信息提高精度，LIS利用VIS的估计结果作为扫描匹配初始值。回环检测首先由VIS识别，再由LIS进一步确认。
两个子系统中任意一个失效了，LVI-SAM仍可以正常工作，这说明它在无纹理和无特征环境下仍然具有高鲁棒性。

二、引言

主要贡献：
1、LVI-SAM是基于因子图实现的紧耦合LVIO框架，并且在位置识别的帮助下实现了多传感器融合和全局优化。
2、LVI-SAM可以通过故障检测绕过失效的子系统，因此对传感器退化具有鲁棒性。
3、不同尺度、平台和环境下，LVI-SAM均能够有效运行。

三、LVI-SAM方法介绍

3.1 系统概述

VIS处理图像、IMU的测量结果和激光雷达测量结果（可选），最小化视觉和IMU测量结果的联合残差实现视觉里程计。LIS通过特征与特征地图的匹配来提取特征并执行激光雷达里程计，特征地图以滑动窗口的方式进行维护，从而提高实时性。最后利用ISAM2，联合优化因子图中的IMU预积分约束、视觉里程计约束、激光雷达里程计约束、和回环约束，解决状态估计问题（上述过程也可以表示为最大后验概率问题）。LIS中的多传感器图优化的目的是减少数据交换并提高系统效率。

3.2 视觉-惯性子系统（VIS）

VIS因子图如图所示：
视觉特征点采用角点检测器提取，并采用KLT光流法追踪。VIS初始化以后，我们利用视觉里程计对激光雷达帧进行配准，并且获取稀疏的深度图用于特征点深度估计。系统在滑动窗口中执行光束法平差，系统状态x可以表示为：
x=[R,p,v,b]
其中R是旋转矩阵，p为平移向量，v为速度，b是包含加速度计零偏和陀螺仪零偏的IMU的零偏向量。

3.2.1 初始化

普通的基于优化的VIO初始化质量不好甚至初始化失败，主要是由于两个原因：1 单目尺度不确定性 2 IMU测量噪声来源多并且加速度激励不容易满足。
为了提高VIS初始化的鲁棒性，首先初始化LIS获得系统状态x和IMU的零偏，然后基于图像帧的时间戳将它们插值并关联到图像帧（注意到此处假定两个相邻的图像关键帧之间的零偏值是不变的），最后用关联值初始化VIS，从而提高VIS初始化的速度和鲁棒性。

3.2.2 特征深度关联

此处与V-LOAM的做法基本相同。
首先将视觉特征点和点云点投影到以相机为球心的单位球上，用极坐标形式表示点的位置，进行下采样。用二维kd-tree搜索每个视觉特征点附近的三个临近点云点，认为从相机光心到特征点（归一化平面上）的直线与三个点云点确定的平面之间的交点的深度值就是特征点的深度值。
为了获取更加密集的点云点投影，我们会将多个激光雷达点云帧叠加在一起，因此在获取最终的深度值之前，要先验证三个点云点之间的距离，如果距离过远，可能导致获取的深度值来自于另一个目标，如上方右图所示，时间tj时，相机理应只能看到前景点，但是由于点云帧叠加，导致它可能搜索到后景点，最终获得的深度值不准确。如果三个点之间的最大距离超过2m，则放弃获取该特征点的深度值。
如图所示，绿色的点为成功关联深度的特征点，可见处于角落部分的特征点常常缺乏特征关联。

3.2.3 失效检测

遇到剧烈运动、光照变化和无纹理环境时，VIS可能无法提取到足够的特征点，进而导致优化发散，VIS系统失效。VIS失效时，估计的IMU零偏会变大。因此在特征点数量小于一定的阈值时，或估计的IMU零偏超过一定阈值时，则报告VIS子系统失效。
失效检测是必要的，这样可以保证VIS失效不会影响LIS的功能，一旦检测到VIS失效，VIS将重新初始化并通知LIS。

3.2.4 回环检测

采用词袋模型DBoW2进行回环检测，对于每个新图像关键帧，提取BRIEF描述子，与之前的描述子进行词袋匹配。把DBoW2返回的回环帧时间戳发送给LIS，由LIS进行进一步的回环验证。

3.3 激光-惯性子系统（LIS）

LIS因子图如图所示：
因子图中包含四种约束：IMU预积分约束，视觉里程计约束，激光里程计约束，回环约束，因子图会对这几种约束进行联合优化。
激光雷达关键帧基于位姿变化的大小，多个激光雷达关键帧共同组成特征地图，特征地图由滑动窗口的方式控制规模。

3.3.1 初值预测

剧烈运动时，激光雷达帧匹配选取的初值（预测值）十分重要。LIS初始化之前与之后预测来源是不同对的。
LIS初始化之前，假设机器人从某位置启动，初速度为0。假设零偏和噪声均为0，对IMU测量值积分，计算两个激光雷达关键帧之间的平移和旋转以作为预测。在速度小于10m/s且角速度小于180°/s时，此方法可以成功初始化LIS。LIS初始化以后，在因子图优化IMU零偏、位姿、速度，并发送给VIS帮助VIS初始化。
LIS初始化以后，预测的来源有两个：校正后的IMU预积分值和VIS的估计结果。VIS结果可用时，优先采用VIS的估计结果。如果VIS失效，则利用IMU获取预测。这种双来源的预测方法提高了多纹理环境和无纹理环境的预测准确性和鲁棒性。

3.3.2 失效检测

激光雷达遇到像上面一样的几何特征丢失的场景下会产生退化。
根据非线性优化迭代过程判断LIS是否失效：当A^TA的最小特征值小于第一次优化迭代是的阈值时，认为LIS失效。（可以这样想，A的奇异值特别小的时候，A^TA接近于病态矩阵或奇异矩阵，导致得到的解不可信）LIS失效后，就不在因子图中添加激光里程计约束了。

四、实验

4.1 消融实验

4.2 数据集实验

最后介绍一下我们最近的几个学习活动：（点击下方标题即可快速获取）
1、实战：基于ORB-SLAM2的管道机器人圆柱约束的位姿估计
管道内单目视觉SLAM尺度漂移较明显，定位挑战很大。学完本项目，你可以动手做到：利用管道的圆柱形结构约束，基于ORB-SLAM2改进地图点位置估计，从而优化SLAM算法精度。详情点击 实战：基于ORB-SLAM2的管道机器人圆柱约束的位姿估计
2、快速入门多传感器融合SLAM：LVI-SAM
LVI-SAM是MIT团队的TixiaoShan等人开源的一个紧耦合的激光雷达视觉惯导SLAM系统，可以实时高精度鲁棒的进行状态估计和建图。哈工大博士带你学习，1个月的学习挑战赛，详情点击 MIT团队开源的这个激光雷达视觉惯导SLAM系统，太香了！
3、快速入门激光雷达惯性SLAM-FastLio2
FAST-LIO2是香港大学火星实验室（MARS）发表在IEEE-RAL和IEEE-TRO的两篇论文，是一种具有高计算效率、高鲁棒性的雷达惯性里程计（LIO）本课程中上海交通大学博士王泽霖推出为期1个月的学习挑战赛第2期，从原理到实战快速掌握，详情点击 香港大学开源的这个激光雷达惯性SLAM，太强了！
4、效果逼近LIO-SAM！带你实现激光雷达SLAM
本项目对ALOAM进行改进，融合众多新技术，ALOAM+ LINK3D + CSF + BALM，纯激光雷达SLAM效果逼近LIO-SAM。详情点击效果逼近LIO-SAM！带你实现激光雷达SLAM：ALOAM+ LINK3D + CSF + BALM
“长按”或“扫描”下方二维码，加入【小六的机器人SLAM圈】即可开始学习~3天内无条件退款~ 购买后务必加客服领取其他权益和加交流群，现在加入免费送近千页5年机器人SLAM答疑手册
全国最大的机器人SLAM开发者社区
继续阅读
阅读原文