Lv3st帝法（LVI-SAM：配置环境、安装测试、适配自己采集数据集）

LVI-SAM是TixiaoShan大佬在他之前LIO-SAM工作基础上耦合了视觉惯性里程计    
    
    
，算法包含雷达惯性里程计模块及视觉惯性里程计模块


     


     


  ，其中视觉惯性里程计采用VINS-MONO
 




 




 
，其实整体设计是为了在雷达退化的场景中   

   

   

，使用视觉里程计定位结果代替雷达退化方向位姿 

     

     

 ，同时利用雷达惯性里程计结果初始化整个视觉惯性里程计系统

  




  




  ，并使用Lidar点云深度信息融合图像数据  


  


  


，雷达惯性里程计中同样使用视觉词袋回环检测结果参与因子图优化.

之前写过一篇有关LIO-SAM安装并适配自己传感器的文章：LIO-SAM：配置环境    
    
    
、安装测试


     


     


  、适配自己采集数据集  
     
     
，后续因为一直没有使用到视觉传感器


   




   




   ，因此一直没有调试LVI-SAM相关算法 



 



 


，现在想要测试一下融合视觉传感器相关算法             ，因此把自己调试的一些问题做一个记录



    




    




   ，也希望其中的某些点能够帮到大家.

其实有关LVI-SAM安装配置












，网上也已经有很多大佬进行了相应的阐述              ，因此在这里不做算法原理记录了



     



     



   ，仅仅记录一下自己调试适配自己数据过程中











，有哪些需要注意的点    
    
    
，然后如何快速适配自己的数据


     


     


  ，我也是菜鸟一个
 




 




 
，文章中如果有出现理解并不正确的地方   

   

   

，还希望大家批评指正.

Paper: https://github.com/TixiaoShan/LVI-SAM/blob/master/doc/paper.pdf

Code : https://github.com/TixiaoShan/LVI-SAM

1. 电脑配置

Ubuntu 18.04 + ROS Melodic + GTSAM 4.0.2 + CERES 1.14.0

2. 环境配置

2.1. ROS Melodic安装

可参考ROS wiki官网教程.

2.2. GTSAM 4.0.2安装

GTSAM官网：https://github.com/borglab/gtsam

git clone https://github.com/borglab/gtsam.git mkdir build && cd build cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. sudo make install -j4

2.3. Ceres 1.14.0安装

Ceres官网：https://github.com/ceres-solver/ceres-solver

git clone https://github.com/ceres-solver/ceres-solver.git mkdir build && cd build cmake .. sudo make install -j4

2.4. 创建工作空间并编译

mkdir ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/TixiaoShan/LVI-SAM.git cd .. catkin_make -j4

3. 运行示例数据

  示例数据Google网盘链接：https://drive.google.com/drive/folders/1q2NZnsgNmezFemoxhHnrDnp1JV_bqrgV?usp=sharing

为了方便下载 

     

     

 ，已转移其中部分数据至百度网盘

  




  




  ，有需要可以使用下方链接获取：

链接: https://pan.baidu.com/s/1xKkva1sHI4amKswnWihKqQ 提取码: 7bem

启动程序运行示例数据：

# 启动LVI-SAM建图节点 roslaunch lvi_sam run.launch # 播放示例数据 rosbag play handheld.bag

4. 适配自己传感器数据

  由于LVI-SAM包含LIO及VIO两个模块  


  


  


，适配自己传感器数据时需要两个模块都进行适配  
     
     
，即保证单独的一个模块针对自己传感器数据都是可稳定运行的, 如果前期做过视觉SLAM的朋友可能更容易调试一些, 调试过程总体可以分为三步：

4.1. 调试LIO模块

  调试LIO模块


   




   




   ，此过程可以在launch文件中注释掉VIO模块节点 



 



 


，仅使用LIO运行数据             ，确保LIO模块是能够稳定运行的



    




    




   ，LVI-SAM系统的LIO模块是在LIO-SAM基础上稍作改动












，代码整体逻辑与LIO-SAM基本一致              ，同样需要雷达数据提供‘ring‘及’time’属性



     



     



   ，调试过程可以参考我的另一篇文章：LIO-SAM：配置环境
 




 




 
、安装测试   

   

   

、适配自己采集数据集











，这里不再赘述;

4.2. 调试VIO模块

  LVI-SAM系统是将VINS-MONO作为一个单独的里程计模块运行的, 可以融合LIO模块信息进行VIO模块系统初始化以及获取Lidar局部点云地图为图像提供深度信息, 也可以独立初始化运行（但是对于平面运动无人车初始化较为困难）; 因此如果觉得联合调试较为困难    
    
    
，则可以当做直接调试VINS-MONO工程即可


     


     


  ，作者留好了相应的接口配置
 




 




 
，只需要在config/params_camera.yaml文件中修改以下参数即可：

# Lidar Params use_lidar: 1 # whether use depth info from lidar or not lidar_skip: 3 # skip this amount of scans align_camera_lidar_estimation: 1 # align camera and lidar estimation for visualization

"use_lidar"参数为 1 则表示使用LIO模块结果初始化VIO系统并且进行图像深度估计   

   

   

，反之则不使用Lidar点云数据及LIO数据.

"align_camera_lidar_estimation"参数则是为了便于同步雷达惯性里程计与视觉惯性里程计显示用的.

4.2.1 配置相机内参

  调试视觉惯性里程计模块最关键的是要做好相机去畸变 

     

     

 ，因此第一步需要做的便是设置相机内参

  




  




  ，调试前需要明确设备的相机模型及畸变模型（针孔 or 鱼眼 or 其它）  


  


  


，然后根据具体设备使用的模型进行相机内参标定, 标定可以使用Kalibr功能包（Kalibr可以标定相机内参 

     

     

 、多相机标定及标定Camera与IMU外参  
     
     
，相关资料网上很多


   




   




   ，这里不做赘述） 



 



 


，标定完成后直接在config/params_camera.yaml文件中修改为自己的相机内参即可             ，具体要修改的相机内部参数部分如下：

1. 鱼眼相机模型（以lvi-sam示例数据为例） model_type: MEI camera_name: camera image_width: 720 image_height: 540 mirror_parameters: xi: 1.9926618269451453 distortion_parameters: k1: -0.0399258932468764 k2: 0.15160828121223818 p1: 0.00017756967825777937 p2: -0.0011531239076798612 projection_parameters: gamma1: 669.8940458885896 gamma2: 669.1450614220616 u0: 377.9459252967363 v0: 279.63655686698144 2. 针孔相机模型（以urban-nav数据集为例） model_type: PINHOLE camera_name: camera image_width: 1920 image_height: 1200 distortion_parameters: k1: -0.109203 k2: 0.063536 p1: -0.003427 p2: -0.000629 projection_parameters: fx: 1086.160899 fy: 1090.242963 cx: 940.067502 cy: 586.740077 4.2.2 配置VIO模块外参

  相机内参设置完成后



    




    




   ，接下来则需要根据真实设备












，调整VIO模块中各个传感器之间外参              ，要更改的外参主要包括lidar-camera

  




  




  、camera-imu  


  


  


、lidar-imu外参



     



     



   ，下面将会对lvi-sam涉及传感器外参部分进行一些解释











，以便修改适配个人传感器：

Camera-IMU外参修改：在Vins-Mono原始代码中已经将外参设置修改放在了yaml文件中    
    
    
，因此可以直接在config/params_camera.yaml文件中更改"extrinsicRotation"以及"extrinsicTranslation"参数： # 以下为IMU To Camera外参 #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 0, 0, -1, -1, 0, 0, 0, 1, 0] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [0.006422381632411965, 0.019939800449065116, 0.03364235163589248]

  接下来要更改Lidar-Camera以及Lidar-IMU外参


     


     


  ，这两部分外参设置需要修改代码
 




 




 
，因为作者根据自己的设备将外参在代码中固定了, 如果不在程序中修改为自己设备的实际外参   

   

   

，则很容易跑飞.

Lidar-IMU外参修改：作者直接在代码中固定了lidar-imu外参（由于作者设备各传感器安装紧凑 

     

     

 ，因此忽略了平移量）

  




  




  ，具体代码实现在’visual_estimator/visualization.cpp’中  


  


  


，代码如下, 如果要设置lidar-imu外参  
     
     
，则需要更改此处代码： tf::Quaternion q_odom_cam(Q.x(), Q.y(), Q.z(), Q.w()); // 这里其实是利用lidar-imu外参


   




   




   ，获取lidar坐标系在VIO系统世界系下坐标 tf::Quaternion q_cam_to_lidar(0, 1, 0, 0); // mark: camera - lidar tf::Quaternion q_odom_ros = q_odom_cam * q_cam_to_lidar; tf::quaternionTFToMsg(q_odom_ros, odometry.pose.pose.orientation); // 用于给LIO系统做位姿预测 pub_latest_odometry_ros.publish(odometry); // TF of camera in vins_world in ROS format (change rotation), used for depth registration // 这里从注释可以看出, 是发送的camera在VIO系统世界系下坐标 



 



 


，只给到特征提取模块进行深度关联 // 但是这里直接将lidar位姿作为camera位姿发布出去了             ，是由于后面深度关联中



    




    




   ，要利用camera视场角做点云筛选 // 而在ros 标准坐标系定义都是以x: front y: left z: up为准












，为了便于后续计算              ，这里直接做了将camera坐标系转换为ros标准系的隐式变换 // 而在作者设备中



     



     



   ，lidar坐标系与ros标准系一致且可以忽略camera与lidar之间的平移量











，因此camera位姿可以直接使用lidar位姿代替 // 如果这里camera与lidar安装偏差较大（旋转和平移）    
    
    
，则此处需要进行较大改动


     


     


  ，需要明确camera标准ros系与lidar坐标系外参 // 这里的一些关系可以由后面特征提取模块作者给出的定义及 lidar_to_cam_ty等外参分析获得. tf::Transform t_w_body = tf::Transform(q_odom_ros, tf::Vector3(P.x(), P.y(), P.z())); tf::StampedTransform trans_world_vinsbody_ros = tf::StampedTransform(t_w_body, header.stamp, "vins_world", "vins_body_ros"); br.sendTransform(trans_world_vinsbody_ros);

  同时在"visual_estimator/initial/initial_alignment.h"中作者也直接在初始构造函数中初始化了Lidar-IMU外参, 如果要设置lidar-imu外参
 




 




 
，此处也需要更改：

odometryRegister(ros::NodeHandle n_in): n(n_in) { // lidar-imu外参   

   

   

，主要作用是将LIO系统lidar位姿转换到VIO系统世界系下 

     

     

 ，并提供给VIO系统进行系统初始化 q_lidar_to_cam = tf::Quaternion(0, 1, 0, 0); // rotate orientation // mark: camera - lidar // VIO系统世界系与LIO系统世界系外参

  




  




  ，固定值即可 q_lidar_to_cam_eigen = Eigen::Quaterniond(0, 0, 0, 1); // rotate position by pi, (w, x, y, z) // mark: camera - lidar // pub_latest_odometry = n.advertise<nav_msgs::Odometry>("odometry/test", 1000); } Lidar-Camera外参修改：lidar-camera外参修改比较麻烦  


  


  


，作者根据自己的设备情况在代码中做了很多隐式转换  
     
     
，主要体现在VIO系统特征提取部分


   




   




   ，并且如果配置参数中选择VIO系统不使用Lidar点云估计图像深度 



 



 


，则此部分外参也无需修改（由于作者设备各传感器安装紧凑             ，因此忽略了平移量）



    




    




   ，最直观的一个lidar-camera外参设置则是yaml文件中的一下部分： # 以下为Camera-Lidar外参












，是lidar坐标系与camera的ros标准系之间的外参              ，可以理解为此外参在lidar-camera外参基础上多做了一步camera坐标系到ros标准系的隐式转换 # lidar to camera ros standard extrinsic lidar_to_cam_tx: 0.05 lidar_to_cam_ty: -0.07 lidar_to_cam_tz: -0.07 lidar_to_cam_rx: 0.0 lidar_to_cam_ry: 0.0 lidar_to_cam_rz: -0.04

  配置完上述部分参数后



     



     



   ，理论上可以正常运行自己的数据了.

4.3. LVI-SAM代码修改

  这里为了方便快速适配自己搭建传感器设备











，修改了一版简单版本    
    
    
，主要特性如下：

将传感器外参设置全部放入yaml文件的代码, 包括Lidar-IMU  
     
     
、Lidar-Camera


   




   




   、Camera-IMU及Camera-ROS standard外参


     


     


  ，所有外参均可以直接在yaml文件中修改, 并且将代码中隐式转换部分进行拆解
 




 




 
，更容易按照自己的设备进行参数配置； 考虑相机坐标系与常规坐标系不同情况   

   

   

，可以直接更改yaml文件适配; 考虑lidar坐标系与ros标准系不同时点云过滤不准确问题 

     

     

 ，更加灵活适配自己传感器； 同时源代码中默认Lidar-IMU-Camera均安装在一起, 因此代码中仅使用了旋转参数, 忽略了平移量, 由于个人设备各传感器安装位置距离偏大, 因此代码中也加入了外参平移量； 支持Kitti数据集运行； 支持Urban-Nav数据集运行； 代码且已上传至Github

  




  




  ，有需要的朋友可以Star一下.

LVI-SAM-Simple：https://github.com/YJZLuckyBoy/LVI-SAM-Simple (此版本只是简单基于lvi-sam修改, 适配雷达品牌较少)

lviorf链接：https://github.com/YJZLuckyBoy/lviorf (适配更多雷达品牌且更容易适配不同的传感器设备)

使用LVI-SAM-Simple适配自己传感器设备时主要注意以下参数修改  


  


  


，params_lidar.yaml’文件修改比较简单  
     
     
，这里不做分析；主要介绍一下’config/params_camera.yaml‘’文件修改


   




   




   ，其实有关相机内参修改直接参考4.2.2节中第一部分 



 



 


，重点外参修改如下: cameraToROSStandard外参修改             ，这个参数表示camera坐标系（通常情况下为x: right y: down z: front）与ros标准系（x: left y: right z: up）之间的变换关系



    




    




   ，通常情况下此外参为固定值












，如果你的相机坐标系满足x: right y: down z: front              ，则此参数不需要修改



     



     



   ，此参数主要是为了后面过滤相机视场角以外点云及深度关联使用. # camera to ros standard frame(front-left-up) extrinsic, Theoretically, it is a fixed value # for lio-sam datasets(front-left-up) cameraToROSStandard: !!opencv-matrix rows: 3 cols: 3 dt: d data: [0, -1, 0, 0, 0, -1, 1, 0, 0] camera-imu外参配置











，此外参直接根据标定结果在’config/params_camera.yaml‘’文件中修改以下参数即可: #Rotation from camera frame to imu frame, imu^R_cam extrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [ 0, 0, -1, -1, 0, 0, 0, 1, 0] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [0.006422381632411965, 0.019939800449065116, 0.03364235163589248] 最后需要修改lidar-imu参数    
    
    
，这里修改比较简单


     


     


  ，直接在’params_lidar.yaml’文件中修改即可： # Extrinsics (lidar -> IMU) extrinsicTrans: [0.0, 0.0, 0.0] extrinsicRot: [-1, 0, 0, 0, 1, 0, 0, 0, -1]

4.3. lviorf运行结果

4.3.1 适配个人无人车设备

  由于调试过程中未进行Lidar与Camera精细化标定
 




 




 
，只是给了一个大概的外参数据   

   

   

，因此精度稍差 

     

     

 ，但是不影响运行结果

  




  




  ，以下是分别使用针孔相机及广角相机适配结果  


  


  


，同时两次数据使用了不同品牌雷达进行了适配.

4.3.1.1 针孔相机 4.3.1.2 广角相机 4.3.2 Kitti数据集 4.3.3 Urban-Nav数据集

4.4. 思考

以上就是自己调试过程中的一些问题记录  
     
     
，希望大家多多指正. 多模态融合SLAM目前是很火的一个方向


   




   




   ，LVI-SAM就是一个典型的多模态融合方案 



 



 


，即视觉惯性里程计模块与雷达惯性里程计模块融合             ，两个模块间既有数据交互融合



    




    




   ，但是当某一个模块失效后也不会影响到另一个模块的运行












，不得不说LVI-SAM是一个很好的多源融合框架              ，基于这个框架后续有一些小的思考和疑问



     



     



   ，在这里简单说一下











，欢迎大家一起讨论.

作者提到后续会在预积分模块融合Lidar里程计 



 



 


、视觉里程计             、预积分进行联合优化    
    
    
，这将是一个更加紧耦合的方案


     


     


  ，如果单一模块出现问题后如何降低相应权重是系统更稳定
 




 




 
，很期待大佬的后续工作；

现有调试尚未融合GPS数据   

   

   

，GPS参与优化后对系统是否有很大影响 

     

     

 ，尤其是比较廉价的GPS设备对系统是否有很大影响

  




  




  ，还需要进一步测试  


  


  


，实际测试后融合GPS仍可达到很好的效果  
     
     
，但是需要更改前端预测逻辑；

LVI-SAM整体融合过程中更多是在Lidar退化时


   




   




   ，使用视觉里程计替代退化方向位姿 



 



 


，因此这个思路好像也可以拓展到其他设备中             ，只要使用的设备在Lidar里程计发生场景退化时



    




    




   ，此设备能够保证相对长时间内稳定定位












，都可以替换视觉里程计模块. 同样地              ，把VINS-MONO替换为ORB-SLAM3应该可以达到同样的效果；

LVI-SAM中丢弃了使用IMU预积分模块预测值作为前端里程计的初始值



     



     



   ，预测值全部来自视觉里程计











，但是如果视觉里程计飞掉    
    
    
，则预测值将会使用纯IMU旋转预测


     


     


  ，这对于低成本或是6轴IMU来说（LIO-SAM可以很容易适配6轴传感器
 




 




 
，支持6轴IMU版本可以使用liorf）是很难得到一个好的预测值的   

   

   

，这里是不是应该优先使用IMU预积分得到的结果 

     

     

 ，而在检测到Lidar里程计退化时再使用视觉里程计进行预测和补偿

  




  




  ，实际测试后发现这种方法仍然是可行的.