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

Ubuntu 18.04 ROS Melodic GTSAM 4.0.2 CERES 1.14.0

可参考ROS wiki官网教程.

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 

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 

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

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

为方便下载，已将部分数据转移到百度网盘，必要时可使用以下链接获取：

链接: https://pan.baidu.com/s/1z112ZOZGC8QXGCEDRW_fCw 提取码: 3f32

启动程序运行示例数据：

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

由于LVI-SAM它包括两个模块：雷达惯性里程计和视觉观想里程计。在适应自己的传感器数据时，需要适应两个模块，即确保单个模块能够稳定地运行自己的传感器数据， 假如前期做过视觉SLAM朋友可能更容易调试， 调试过程一般可分为三个步骤：

调试雷达惯性里程计模块，此过程可以注释掉视觉惯性里程计模块，仅使用雷达惯性里程计运行数据，确保雷达惯性模块是能够稳定运行的，LVI-SAM雷达惯性里程计模块LIO-SAM基本上稍作改动，但基本一致，雷达数据也需要提供‘ring‘及’time’属性，调试过程可一篇文章：LIO-SAM：这里不再重复配置环境、安装测试和适应自己的数据集；

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"参数用于同步雷达惯性里程计和视觉惯性里程计的显示.

调试视觉惯性里程计模块最重要的是做好相机畸变，所以第一步是设置相机内部参考，调试前明确设备的相机模型和畸变模型（针孔 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 

最后，调整视觉惯性里程计模块中各传感器之间的外参Camera与IMU可直接修改外参config/params_camera.yaml文件中更改"extrinsicRotation"参数：

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

# 以下为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

上述外参参数修改完成后还没有结束，由于LVI-SAM中作者将一些坐标变换直接按照使用设备设置为了固定参数，因此如果不在程序中修改为自己设备的实际外参，则很容易出现跑飞现象， 这其中涉及的主要是Lidar与Imu外参设置.

例如"src/visual_odometry/visual_estimator/utility/visualization.cpp"中作者直接设定好了外参:

    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"中作者直接在初始构造函数中初始化了外参:

    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<nav_msgs::Odometry>("odometry/test", 1000);
    }

其实上述外参均是ImuToLidar的外参, 因为VINS-MONO还是以IMU系为世界系的, 整体坐标系转换是ImuToLidar的外参，因此适配自己设备时需要修改上述两个文件中的外参值，更改为自己IMU与Lidar外参. 这里为了方便快速适配自己传感器设备，自己改了一版外置传感器外参接口的代码且已上传至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"文件中将"lcExtrinsicTranslation"参数改为自己设备的Lidar与Camera外参，这里以示例数据为例，可直接将外参设置为:

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

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

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

以上就是自己调试过程中的一些问题记录，希望大家多多指正. 多模态融合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里程计退化时再使用视觉里程计进行预测和补偿，实际测试后发现这种方法仍然是可行的.