作者丨Alvin一路向东@知乎

来源丨https://zhuanlan.zhihu.com/p/463778905

编辑丨3D视觉工坊

前言

本文是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 数据集实验

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

14.ROS2从入门到精通：理论与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~