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涉及传感器外参部分进行一些解释，以便修改适配个人传感器：

1. 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外参，这两部分外参设置需要修改代码，因为作者根据自己的设备将外参在代码中固定了, 如果不在程序中修改为自己设备的实际外参，则很容易跑飞.

2. 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);
    }

3. 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节中第一部分，重点外参修改如下:

1. 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]

2. 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]

3. 最后需要修改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里程计退化时再使用视觉里程计进行预测和补偿，实际测试后发现这种方法仍然是可行的.