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包含雷达惯性里程计及视觉惯性里程计两个模块，适配自己传感器数据时需要两个模块都进行适配，即保证单独的一个模块针对自己传感器数据都是可稳定运行的, 如果前期做过视觉SLAM的朋友可能更容易调试一些, 调试过程总体可以分为三步：

4.1. 调试雷达惯性里程计模块

调试雷达惯性里程计模块，此过程可以在launch文件中注释掉视觉惯性里程计模块节点，仅使用雷达惯性里程计运行数据，确保雷达惯性模块是能够稳定运行的，LVI-SAM雷达惯性里程计模块在LIO-SAM基础上稍作改动，但是基本一致，同样需要雷达数据提供‘ring‘及’time’属性，调试过程可以参考我的另一篇文章：LIO-SAM：配置环境、安装测试、适配自己采集数据集，这里不再赘述;

4.2. 调试视觉惯性里程计模块

LVI-SAM中VINS-MONO是作为一个单独的里程计模块进行运行的, 可以融合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 则表示使用lidar里程计初始化VINS系统并且进行图像深度估计，反之则不使用Lidar点云数据及雷达惯性里程计数据.

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

4.2.1 配置相机内参

调试视觉惯性里程计模块最关键的是要做好相机去畸变，因此第一步需要做的便是设置相机内参，调试前需要明确设备的相机模型及畸变模型（针孔 or 鱼眼 or 其它），然后根据具体设备使用的模型进行相机内参标定, 标定可以使用Kalibr功能包（Kalibr可以标定相机内参、多相机标定及标定Camera与IMU外参，相关资料网上很多，这里不做赘述），标定完成后直接在config/params_camera.yaml文件中修改为自己的相机内参即可，具体要修改的相机内部参数部分如下：

# camera model
# 相机模型
model_type: MEI
camera_name: camera

# Mono camera config
# 相机内参
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

4.2.2 配置视觉里程计模块外参

最后要根据真实设备，调整视觉惯性里程计模块中各个传感器之间外参，要更改外参主要包括lidar-camera、camera-imu、lidar-imu外参.

1. 有关Camera-IMU外参修改可以直接在config/params_camera.yaml文件中更改"extrinsicRotation"以及"extrinsicTranslation"参数：

# 以下为Camera-IMU外参
#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-Camera外参旋转部分在yaml文件中设置为单位矩阵即可，但是平移参数需要根据设备真实位置进行设置：

# 以下为Camera-Lidar外参
# lidar to camera 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

但是上述设置Lidar-Camera外参还没有完全设置成功，还需要在"src/visual_odometry/visual_feature/feature_tracker.h"文件中对如下代码利用设备真实外参对点云做变换（源代码中作者直接做了转换）:

    // convert points from 3D to 2D
    // point from lidar frame to camera frame
    Eigen::Vector3d p_3d(-depth_cloud_local->points[i].y,
                          -depth_cloud_local->points[i].z,
                          depth_cloud_local->points[i].x);

3. Lidar-Imu外参设置均需要在代码中修改, 例如"src/visual_odometry/visual_estimator/utility/visualization.cpp"中作者直接设定好了Lidar-IMU外参:

    tf::Quaternion q_odom_cam(Q.x(), Q.y(), Q.z(), Q.w());
    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);
    pub_latest_odometry_ros.publish(odometry);

同时"src/visual_odometry/visual_estimator/initial/initial_alignment.h"中作者也直接在初始构造函数中初始化了Lidar-IMU外参:

    odometryRegister(ros::NodeHandle n_in):
    n(n_in)
    {
        q_lidar_to_cam = tf::Quaternion(0, 1, 0, 0); // rotate orientation // mark: camera - lidar
        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("odometry/test", 1000);
    }

因此适配自己设备时需要修改上述两个文件中的外参值, 更改为自己Lidar-IMU外参, 源代码中注释虽然注释说明外参为Lidar-Camera外参, 但其实上述外参均是Lidar-IMU的外参, 因为VINS-MONO是以IMU系为世界系的, 位姿输出均为IMU Body系位姿.

4. 这里为了方便快速适配自己搭建传感器设备，修改了一版将传感器外参设置全部放入yaml文件的代码, 包括Lidar-IMU、Lidar-Camera、Camera-IMU外参设置均可以直接在yaml文件中修改, 同时源代码中默认Lidar-IMU-Camera均安装在一起, 因此代码中仅使用了旋转参数, 未使用平移量, 由于自己设备Lidar-Camera安装位置距离偏大, 因此代码中也加入了外参平移量, 代码且已上传至Github，有需要的朋友可以Star一下:

Github链接：https://github.com/YJZLuckyBoy/LVI-SAM-Simple

使用修改代码适配自己传感器设备时需要修改三处参数，一处是在"config/params_camera.yaml"文件中将"imu2LidarExtrinsicRotation"参数改为自己设备的Lidar-IMU外参，这里以示例数据为例，可直接将外参设置为:

# imu to lidar extrinsic
imu2LidarExtrinsicRotation:  !!opencv-matrix
        rows: 3
        cols: 3
        dt: d
        data:  [-1,  0,  0,
                  0, 1,  0,
                  0,  0,  -1]

另一处是在"config/params_camera.yaml"文件中将"lcExtrinsicRotation"和"lcExtrinsicTranslation"参数改为自己设备的Lidar-Camera外参，这里以示例数据为例，可直接将外参设置为:

# lidar to camera extrinsic
lcExtrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 0, -1, 0, 
           0, 0, -1, 
           1, 0, 0]

lcExtrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.05, -0.07, -0.07]

最后一处是在"config/params_camera.yaml"文件中将"extrinsicRotation"和"extrinsicTranslation"参数改为自己设备的IMU-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.99, 0.05, 0.2475]

4.3. 适配数据运行结果

由于调试过程中未进行Lidar与Camera精细化标定，只是给了一个大概的外参数据，因此精度稍差，但是不影响运行结果，以下是分别使用针孔相机及广角相机适配结果，同时两次数据使用了不同品牌雷达进行了适配.

4.3.1 针孔相机

4.3.2 广角相机

4.4. 思考

以上就是自己调试过程中的一些问题记录，希望大家多多指正. 多模态融合SLAM目前是很火的一个方向，LVI-SAM就是一个典型的多模态融合方案，即视觉惯性里程计模块与雷达惯性里程计模块融合，两个模块间既有数据交互融合，但是当某一个模块失效后也不会影响到另一个模块的运行，不得不说LVI-SAM是一个很好的多源融合框架，基于这个框架后续有一些小的思考和疑问，在这里简单说一下，欢迎大家一起讨论.

• 作者提到后续会在预积分模块融合Lidar里程计、视觉里程计、预积分进行联合优化，这将是一个更加紧耦合的方案，如果单一模块出现问题后如何降低相应权重是系统更稳定，很期待大佬的后续工作；

• 现有调试尚未融合GPS数据，GPS参与优化后对系统是否有很大影响，尤其是比较廉价的GPS设备对系统是否有很大影响，还需要进一步测试；

• LVI-SAM整体融合过程中更多是在Lidar退化时，使用视觉里程计替代退化方向位姿，因此这个思路好像也可以拓展到其他设备中，只要使用的设备在Lidar里程计发生场景退化时，此设备能够保证相对长时间内稳定定位，都可以替换视觉里程计模块. 同样地，把VINS-MONO替换为ORB-SLAM3应该可以达到同样的效果；

• LVI-SAM中丢弃了使用IMU预积分模块预测值作为前端里程计的初始值，预测值全部来自视觉里程计，但是如果视觉里程计飞掉，则预测值将会使用纯IMU旋转预测，这对于低成本或是6轴IMU来说（LIO-SAM可以很容易适配6轴传感器）是很难得到一个好的预测值的，这里是不是应该优先使用IMU预积分得到的结果，而在检测到Lidar里程计退化时再使用视觉里程计进行预测和补偿，实际测试后发现这种方法仍然是可行的.