多传感器融合 SLAM LVI-SAM
目录
LVI-SAM 简介
A. 系统概述
B. 视觉惯导系统
C.雷达惯导系统
LVI-SAM 安装编译
编译 LVI-SAM
常见问题
LVI-SAM 工程化建议
LVI-SAM 简介
源码地址：https://github.com/TixiaoShan/LVI-SAM
如无法下载，换用 gitee 版本：https://gitee.com/inf_lee/LVI-SAM
改进版源码地址：https://github.com/Cc19245/LVI- SAM - Easyused
建议在实际项目部署中采用优化后的代码，在初学阶段则采用基础版本的代码
gitee 版本：https://gitee.com/cuge1995/LVI-SAM-modified
论文地址：https://github.com/TixiaoShan/LVI-SAM/blob/master/doc/paper.pdf
本文开发了一个紧密集成的雷达视觉惯导SLAM系统，并实现了实时高精度鲁棒运行。
状态估计与建图方面，在LVI-SAM方法中，其构建基础为因子图，并且包含两个子系统：其中一部分为视觉惯导系统。
与一个激光惯导系统协同工作的两个关键子系统采用了紧密耦合的技术：其中视觉惯导系统的输出通过激光惯导系统的估计来辅助实现精确定位。
借助计算技术 实现初始化过程 ，并同时通过激光雷达的数据采集为视觉特征获取深度信息以提高定位准确性。此外，在后续阶段中采用激光惯导系统
采用视觉惯性估计值来确定初始参数对帧进行匹配处理，并通过视觉手段实现闭环检测并将结果传递到激单元中处理
为了实现光惯导系统的优化功能，在激光惯导与视觉惯导子系统出现故障的情况下，LVI-SAM 系统依然能够正常运行。
作业，并显著提升了系统在缺少纹理或特征的场景中的鲁棒性。该系统基于所选数据集实现了目标定位与实例分割任务的表现
进行了测试，取得了很好的效果。
A. 系统概述
系统采集包括3D激光点云、单目图像以及IMU信息的输入数据。视觉系统利用来自图像和IMU信息进行处理。
达点云可以选择作为输入的数据源。通过减少IMU和视觉数据之间的测量偏差来实现对环境运动状态的有效估计。激光里程计
经过减小检测出的线面特征与特征图之间的差距这一过程完成。特征图则被存储在一个滑窗中用于实时监控。
行。最后的状态估计问题可以转化为一个最大后验概率（MAP）优化问题。利用iSAM2库来
在优化因子图中进行处理的是IMU预积分计算、视觉里程计输出以及激光里程计测量结果，并结合闭环回环中的约束误差来计算最终的残差。
LIS 中采用的多传感器图优化旨在减少数据交换并提高系统效率。
B. 视觉惯导系统
视觉惯导的 pipeline 如图所示
视觉特征基于角点检测模块，并采用Kanade-Lucas-Tomasi算法进行追踪。我们通过视觉里程计实现LiDAR数据的精确对齐。
通过获取一个稀疏深度图来实现特征的深度填补视觉惯导初始化过程。该系统在滑动窗口内进行 bundle adjustment 计算
中状态 X 可以写成：
R 是 SO3 表示旋转矩阵，p 表示位置，v 表示速度，b=[b_a, b_w]是 imu 的偏置。变换矩阵
T 代表 SE3 羾群中从机体坐标系到世界坐标系的变换操作。在下文部分将详细阐述VIS的初始化过程及其特征深度计算方法。
计。读者们可以读 VINS-Mono 的原文来了解细节。
基于优化的 VIO 系统 初始化过程中 因系统中存在很强的非线性而导致初始化过程易发散 这一现象对系统的性能有着重要影响 初始化质量不仅取决于系统参数设置 还受到算法设计精度等多种因素的影响
关键要素包括起始阶段的传感器运动学信息以及惯性测量单元参数的精度水平。通过实际实验，在VINS系统中我们观察到其性能表现优异。
当系统处于低速或恒定速度运行的状态时，初始化可能会遇到困难。这种情况归因于加速激励不足
观。IMU 的参数包含渐变的 bias 和高斯白噪声。
为了提高初始化过程的稳定性（即Vis初始化）, 本文采用LIS系统来估计系统的状态变量X以及IMU校正参数b
由于可以通过雷达观测手段获取深度数据。随后我们通过初始化LIS系统来获取x和b值。
They align the laser frames and visual frames through time synchronization using interpolation. Assuming that the IMU bias exists between two image keyframes pairwise.
是一个定值。在此基础上 采用 LIS 系统初始化得到的 x 和 b 作为 VIS 的初始值 ，能显著提高初始化速度。
度和鲁棒性。
基于VIS初始化的结果，在此基础上我们通过视觉里程计实现了视觉帧与雷达帧的对准。在上述方法建立起来的框架下
在当前阶段利用3D Lidar技术进行扫描时会捕捉到较为分散的空间点为了提升空间感知效果我们采用多帧激光雷达捕捉的数据进行融合处理从而形成较为密集且连续的空间深度信息
图。为了实现特征与深度值之间的关联性，在相机中心设置一个单位圆形区域进行特征提取。
上 。随后对深度点执行降采样，并采用极坐标方法确保样本密度均匀一致。采用二维
利用极坐标KD-tree（Polar KD-Tree）来定位视觉特征点周围的三个邻近关键点；随后将每个特征点的深度计算基于相机参数。
连接中心与特征点的长度值，在笛卡尔坐标系中与该三个深度点确定的平面存在交集。
视化如下图 a 所示，特征深度是虚线的长度。
为了进一步验证关联区域的空间关系中的相互作用程度 我们评估了特征点与最近三个相关区域之间的距离 其原因在于
在对激光雷达捕获的不同时间戳帧进行叠加时，可能会导致来自不同物体的深度模糊。这种现象可通过图示来说明
位于图 3(b) 中的位置，在时间 t_i 处观察到的深度点被标记为绿色。相机在时间点 t_j 移动至新的位置，并对周围情况进行观测
实现了灰度深度标记。然而，在 t_i 时刻观测到的被灰度框选的点，在 t_j 时刻可能存在未处理的情况。
可以看到这一现象。这表明利用来自不同对象的深度点来连接特征深度会导致估算出现偏差。通过 检查特
征周围深度点间的最远距离 ，若两特征间最远间距超过 2米，则无需对这些特征间进行数据配准。
3）故障诊断：当遇到剧烈的运动变化时，若发生光照发生变化或环境缺乏纹理特征，则会使得VIS系统失效。
因为在这种场景中被跟踪的特征点数量会显著减少而导致优化过程无法正常进行；当 VIS 系统失效时
败的时候会导致IMU的偏差极大,因此当VIS系统追踪到的特征点数量少于预设阈值或者IMU估算出现偏差时
计算得出 bias 超过了某个指标时，我们判定为VIS系统出现故障。有意识地采取故障检测措施在本研究中
对于系统的正常运行至关重要的是使其不中断以确保在发生故障时不会影响 LIS 系统的正常运行 当故障被检测到时 VIS 将重新初始化以恢复操作并发出警报
LIS 。
闭环检测：本文采用 DBoW2 方法实现闭环检测过程，在每次接收的新图像关键帧中提取 BRIEF 描述子。
描述子不仅与他进行配对，并将其与原有检测所得的描述子进行匹配。基于DBOW2算法识别出的闭环候选帧的时间戳信息会被提供给后续处理流程。
LIS 系统来做进一步的验证
C.雷达惯导系统
从图中可以看出，在本研究中所提出的雷达惯导系统是以LIO-SAM为基础的，并主要依赖因子图来进行全局位姿优化
化 。
主要有四种类型：IMU 的预积分算法、视觉里程计的工作原理、雷达定位技术以及闭环检测系统的设计方案
基于因子图框架中的优化模型设计需求，在本系统中构建了一个新的约束机制，并将其纳入到整体优化流程中作为重要组成部分。该系统的核心组件之一是基于扫描匹配算法设计的激光雷达里程计系统，在这一模块中采用了一种全新的处理方式：通过结合多传感器数据信息进行实时定位与导航计算，并实现了对环境动态变化的有效响应机制。
实现关键帧与全局特征图之间的对应关系。在闭环检测中所使用的约束候选帧最初是由VIS系统生成的，并随后采用特定的扫描策略完成相应的匹配过程
为了实现进一步优化的目的，本文通过构建特征图来存储激光雷达的关键帧滑动窗口，并以此确保了有限计算的需求
复杂度问题。当机器人位置或姿态变化达到或超过设定阈值时,系统会选择一个新的激光雷达关键帧来更新映射关系,从而丢弃那些不在当前关键帧内的数据
中间使用的普通激光雷达帧。在选择了新的激光雷达关键帧之后, 将被作为节点加入到
在因子图中采用这种方法插入关键帧点，在控制内存使用率的同时保持地图密度，并且能够实现时间分辨率与空间分辨率之间的平衡
有助于维持相对稀疏的因子图用于实时优化。可通过查阅LIO-SAM的相关论文来深入了解细节。在此基础上，
下各节中，本文重点介绍提高系统鲁棒性的新策略。
初始值估计：经过实验研究发现，在连续的扫描匹配中起着关键作用的是初始值设定，在复杂度高的情况下表现尤为突出。
在运动的情况下，在LIS系统初始化前后的初始值来源存在差异；当LIS系统初始化时
假定机器人起始状态保持静止，并且接着假定 IMU 的偏差与噪声均为零水平。随后会对 IMU 的原始测量数据进行积分运算。
通过利用两个雷达捕捉到的关键帧位置与姿态数据作为 scan-match 的初始估计
在具有挑战性的环境中（初始速度低于每秒10米、角速度低于每秒180度）启动系统。当 Lis 系统
初始化完成后，在因子图中我们估算IMU的偏差、机器人位姿及速度。随后将这些量传递给接收端。
VIS 系统中完成其初始化。
当LIS系统初始化完成后，我们可以通过两种途径确定 Scan-match 初始值：IMU 的积分
与 VIS 系统 相关联。当 VIS 系统 的里程计能够输出位姿时，则将其作为初始值；如果 VIS 系统 发生故障报告，则会停止后续操作
时
了初值的精度和鲁棒性。
虽然激光雷达能够获取场景中非常遥远区域的详细信息但也会在某些情况下导致问题
由于扫描匹配未能成功（致扫描匹配失败），如下图所示。本系统新增了对LIS故障的检测能力（本系统增加了对 LIS 故障的检测），当LIS故障发生时（当故障发生时），激光里程（激光里程）将被记录（激光里程）。
计不会添加到因子图中
LVI-SAM 安装编译
https://github.com/TixiaoShan/LVI-SAM
ROS
请参考前述章节
GTSAM
源码安装
git clone https://github.com/borglab/gtsam.git
cd gtsam
mkdir build && cd build
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
sudo make install -j4
Ceres Solver
源码安装
sudo apt-get install -y libgoogle-glog-dev
sudo apt-get install -y libatlas-base-dev
wget -O ~/Downloads/ceres.zip https://github.com/ceres-solver/ceres
solver/archive/1.14.0.zip
cd ~/Downloads/ && unzip ceres.zip -d ~/Downloads/
cd ~/Downloads/ceres-solver-1.14.0
mkdir build && cd build
cmake ..
sudo make install -j4
OpenCV
源码安装
PCL
源码安装
源码下载
GitHub提供了有关PointCloudLibrary/pcl-1.9.0的详细下载链接：https://github.com/PointCloudLibrary/pcl/releases/tag/pcl-1.9.0
拖到最后，下载 tar.gz 版本的
解压、编译、安装
解压安装包
tar -zxvf pcl-pcl-1.9.0.tar.gz
cd pcl-pcl-1.9.0
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
make -j4 # 编译比较慢，耐心等待
sudo make install
编译 LVI-SAM
cd ~/catkin_ws/src # 替换为自己的 catkin_ws 目录
git clone https://github.com/TixiaoShan/LVI-SAM.git
cd ..
catkin_make
https://www.yuque.com/docs/share/fd83d3bc-8881-4214-871a-d5838e6d039f
数据集运行
231
cvlife.net source devel/setup.bash
roslaunch lvi_sam run.launch
#下载 demo 数据集：
https://drive.google.com/drive/folders/1q2NZnsgNmezFemoxhHnrDnp1JV_bq
rgV?usp=sharing
#另启一个终端，并输入
rosbag play handheld.bag
常见问题
1.编译报错：
/usr/bin/ld: 找不到 -lBoost::timer
collect2: error: ld returned 1 exit status
CMakeFiles/lio_sam_imuPreintegration.dir/build.make:759: recipe for
target 'devel/lib/lio_sam/lio_sam_imuPreintegration' failed
make[2]: *** [devel/lib/lio_sam/lio_sam_imuPreintegration] Error 1
CMakeFiles/Makefile2:67: recipe for target
'CMakeFiles/lio_sam_imuPreintegration.dir/all' failed
make[1]: *** [CMakeFiles/lio_sam_imuPreintegration.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
解决办法：
找到 CMakeLists 文件，在第 31 行后添加：
find_package(Boost REQUIRED COMPONENTS timer)
2.主机性能无法实时运行 bag
解决办法：降低播放 bag 速度
rosbag play handheld.bag -r 0.5
LVI-SAM 工程化建议
参照 https://github.com/Cc19245/LVI- SAM - Easyused
该项目做的改进包括：
引入外部参数读取函数。传统的代码假设IMU系与LiDAR系之间不存在平移与旋转关系，在实际应用中这种情况是常见的
实际应用场景中两者未必严格重合
采用 MahonyAHRS算法来实现四元数的计算。通过这种方案能够有效避免对九轴IMU的依赖关系，并进而使得六轴IMU系统能够正常工作
IMU
- 加入了对雷达环进行计算。某些雷达格式不包含环导致原版代码无法运行。并增加了该处的处理逻辑
步骤可以让这些雷达同样可以运行
4.优化了相机与雷达之间的深度关联机制（原版采用了雷达坐标系 xyz轴对应前左上方）
5.适配了不同的数据集，包括 M2DGR/Kitti/Urbanav
roslaunch lvi_sam M2DGR.launch
rosbag play gate_01.bag
其中坐标系和点云注册部分的修改参照：
lvisam 原理与代码精讲：
https://cvlife.net/p/t_pc/goods_pc_detail/goods_detail/term_638d4e4d47734_pNkljh?produ
ct_id=term_638d4e4d47734_pNkljh