LIO-SAM是IROS 2020的一篇论文，目前已经开源，作者曾发表过Lego-loam，整体框架与Lego-loam结构相同，但是可读性感觉要比Lego-loam好的多，并添加了gps因子且真正融合了imu。

网上也已经出现了很多关于解读LIO-SAM的文章，但是配置LIO-SAM并运行自己数据集的教程很少，所以本文也是记录自己踩坑的过程，希望能够对大家有用。我也是菜鸟一个，文章中如果有出现理解并不正确的地方，还希望大家批评指正。

Paper：https://github.com/TixiaoShan/LIO-SAM/blob/master/config/doc/paper.pdf

工程源码：https://github.com/TixiaoShan/LIO-SAM

环境配置及安装、运行

1、首先安装ros kinetic，可以按照wiki官网教程安装。

2、安装好ros后，还需要安装一些依赖功能包

sudo apt-get install -y ros-kinetic-navigation
sudo apt-get install -y ros-kinetic-robot-localization
sudo apt-get install -y ros-kinetic-robot-state-publisher 

3、安装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

4、创建好工作空间catkin_ws，将lio-sam源码下载到catkin_ws/src目录下编译

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

5、运行官方数据集进行测试

LIO-SAM的github首页提供了很多的数据集，但是这些数据集都是在Google网盘中，国内下载很慢或者根本下载不了，但是好在有大神已经做了搬运，大家可以去下面的地址中下载，同时该博主也写了关于LIO-SAM的安装配置教程。

LIO_SAM实测运行，论文学习及代码注释[附对应google driver数据]

上述博客中分享了三个官方数据集：casual_walk_2.bag、outdoor.bag、west.bag。

如果运行casual_walk_2.bag不需要修改任何参数，使用默认文件即可；如果要运行其它两个数据集，则需要做如下修改：

imuTopic: "imu_raw"  改为 imuTopic: "imu_correct"
extrinsicRot 和 extrinsicRPY 设为单位矩阵

其中extrinsicRot表示imu->lidar的坐标变换, 用于旋转imu坐标系下的加速度和角速度到lidar坐标系下, extrinsicRPY则用于旋转imu坐标系下的欧拉角到lidar坐标下, 由于lio-sam作者使用的imu的欧拉角旋转方向与lidar坐标系不一致, 因此使用了两个旋转不同, 但是大部分的设备两个旋转应该是设置为相同的。

同时GitHub主页还提供了验证GPS的数据集(park.bag)，如果要使用GPS数据，则需要进行如下修改：

gpsTopic: "odometry/gpsz"  改为 gpsTopic: "odometry/gps"  
useImuHeadingInitialization: false 改为 useImuHeadingInitialization: true   # 如果使用gps数据这里一定要设置为true，不然可能会跑飞

为了记录测试过程，将GitHub给出的示例数据集做了转移，并将相应的配置参数文件与相对应的数据集放在了一起，有兴趣的朋友可以直接下载数据集和对应的配置参数使用。

链接：https://pan.baidu.com/s/1MqQD22d4sA3iUszlWg3C8Q 提取码：2eyj

启动程序运行示例数据：

# 启动lio-sam功能包
roslaunch lio_sam run.launch
# 播放数据集
rosbag play outdoor.bag

工程参数修改及问题解决

1、如果想要保存地图，需要对config/params.yaml文件进行如下修改

# 保存地图设置为true
savePCD: true                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
# 设置地图保存路径
savePCDDirectory: "自己设置的路径"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

更改了配置文件后，还需更改一下_TIMEOUT_SIGINT参数，否则可能造成地图保存失败(这是由于ros会在_TIMEOUT_SIGINT秒后关闭ros节点，但是地图过大时，保存地图会花费一些时间，如果_TIMEOUT_SIGINT太小，很可能造成地图还未保存，节点就已经关闭了，所以需要适当调高_TIMEOUT_SIGINT值)，具体方法如下：

sudo gedit /opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/nodeprocess.py

找到_TIMEOUT_SIGINT并调整数值(默认15s，我的参考值60s)

参考github：https://github.com/TixiaoShan/LIO-SAM/issues/3

2、编译LIO-SAM功能包时遇到错误提示：

static assertion failed: Error: GTSAM was built against a different version of Eigen

这是由于gtsam自带eigen版本与系统ros中自带eigen版本之间冲突问题，需要修改gtsam包的CMakeLists.txt文件，使其编译时使用系统eigen，然后重新编译安装gtsam。

在gtsam的CMakeLists.txt文件中找到：

if(GTSAM_USE_SYSTEM_EIGEN)
     find_package(Eigen3 REQUIRED)
     …

在 if(GTSAM_USE_SYSTEM_EIGEN) 上方添加一句：

set(GTSAM_USE_SYSTEM_EIGEN ON)

然后重新编译安装GTSAM即可。

参考：https://blog.csdn.net/unlimitedai/article/details/107378759#t1
https://github.com/TixiaoShan/LIO-SAM/issues/3
https://blog.csdn.net/moyu123456789/article/details/107058418/

Lidar + IMU方案运行自己采集数据

以下是适配过程中一些较为重要的注意点，由于lio-sam源码中使用了参数"ring"和"time"，在lio-sam系统启动后，会在imageProjection.cpp的cachePointCloud函数中检查输入点云数据是否包含这两项参数，如果没有，则程序会报错。

        // check ring channelstatic int ringFlag = 0;if (ringFlag == 0){ringFlag = -1;for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i){if (currentCloudMsg.fields[i].name == "ring"){ringFlag = 1;break;}}if (ringFlag == -1){ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");ros::shutdown();}}// check point timeif (deskewFlag == 0){deskewFlag = -1;for (auto &field : currentCloudMsg.fields){if (field.name == "time" || field.name == "t"){deskewFlag = 1;break;}}if (deskewFlag == -1)ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");}

这两个参数分别表示一个激光点所属的线束序号和当前激光点在一帧激光点云的相对扫描时间(例如一个16线激光雷达，ring代表了激光点在竖直方向上所属的线束序号，而time表示当前激光点相对于当前激光帧第一个激光点的扫描时间)。

如果你使用的激光雷达，采集的数据中包含参数"ring"和"time"，那么是不需要进行源码改动的，只需要改动config/params.yaml中的话题名及extrinsicRot和extrinsicRPY矩阵即可(extrinsicRot和extrinsicRPY矩阵如果设置不正确则建出的地图或出现重叠)。

自己使用其它品牌雷达采集的点云数据中往往没有“ring”和”time“参数，因此需要自己在源码中进行修改，适配自己的数据。

• 对于"ring"参数，我们可以参考lego-loam的做法，利用公式求出当前激光点所属线束序号

// 计算竖直方向上的角度（雷达的第几线）
float verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;// rowIdn计算出该点激光雷达是竖直方向上第几线的
// 从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)
int rowIdn = (verticalAngle + ang_bottom) / ang_res_y;

• 对于“time“参数，其实这个参数的作用是记录每个点的扫描时间，用于消除点云运动畸变，因此如果暂时不考虑点云运动畸变，则可以将与time相关代码和函数注释，不影响程序运行; 如果必须考虑点云运动畸变，则可以参考lego-loam中的方法，利用公式求出每个激光点的扫描时间

float startOri = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
float endOri = -atan2(laserCloudIn->points[cloudSize - 1].y,laserCloudIn->points[cloudSize - 1].x) + 2 * M_PI;if (endOri - startOri > 3 * M_PI) {endOri -= 2 * M_PI;} else if (endOri - startOri < M_PI) {endOri += 2 * M_PI;
}bool halfPassed = false;// range image projection
for (int i = 0; i < cloudSize; ++i)
{float ori = -atan2(laserCloudIn->points[i].y, laserCloudIn->points[i].x);if (!halfPassed) {if (ori < startOri - M_PI / 2) {ori += 2 * M_PI;} else if (ori > startOri + M_PI * 3 / 2) {ori -= 2 * M_PI;}if (ori - startOri > M_PI) {halfPassed = true;}} else {ori += 2 * M_PI;if (ori < endOri - M_PI * 3 / 2) {ori += 2 * M_PI;} else if (ori > endOri + M_PI / 2) {ori -= 2 * M_PI;}}float relTime = (ori - startOri) / (endOri - startOri);....................................
}

运行自己数据时未使用点云去畸变，并且按照自己设备中传感器安装关系设置了extrinsicRot和extrinsicRPY矩阵(一定要设置正确，否则各种跑飞和抖动现象)。

• 修改版本1：为了方便快速适配不同雷达数据, 以及便于扩展适配雷达驱动不包含ring、time参数的数据, 更改了一版简单的适配其他厂商雷达数据以及自动解算ring、time属性的代码, 现已上传到github, 有需要的朋友可以Star一下, 具体适配自己设备需要修改的参数可以参考README.md文件.

Github链接: https://github.com/YJZLuckyBoy/LIO-SAM-Modified

• 修改版本2：为了后续想要适配更多激光雷达（机械/固态/多雷达），而不用考虑特征提取模块的通用性，因此参考FAST-LIO2更改了一版移除LIO-SAM特征提取模块的代码，直接使用全部点云数据进行里程计解算，并且代码适配了速腾雷达、6轴IMU、9轴IMU数据，目前测试LIO-SAM提供数据（Velodyne、Livox、Ouster）及UrbanNavDataset数据集均没有问题，并且进行一些性能测试和对比，现已上传到github, 有需要的朋友可以Star一下, 具体适配自己设备需要修改的参数可以参考README.md文件.

建图算法链接: https://github.com/YJZLuckyBoy/liorf

定位算法链接: https://github.com/YJZLuckyBoy/liorf_localization

B站优化算法视频：liorf系列算法效果演示

Lidar + IMU + GPS方案适配自己采集数据

要添加自己的GPS数据作为约束，首先要清楚lio-sam中是如果对原始的IMU数据和GPS数据进行转换的，这里一开始并不是太清楚，后来经过很多次单个节点调试才搞明白。

在lio-sam中的params.yaml文件中可以看到，里面设置了两种IMU话题类型：“imu_raw” 和 “imu_correct”，其中"imu_raw"是IMU原始输出，而"imu_correct"是作者通过更改IMU驱动发布的REP-105格式的数据，并且将输出的参考坐标系改为了"base_link"。

如果想要适配自己的GPS数据，首先确保你具有"sensor_msgs/NavSatFix"类型的GPS数据，然后需要对一些配置文件进行修改，首先在params.yaml文件中要修改下面的一些参数：

lio_sam:pointCloudTopic: "yourself_lidar_topic"               # Point cloud dataimuTopic: "yourself_imu_topic"                         # IMU datagpsTopic: "odometry/gps"    useImuHeadingInitialization: true
ekf_gps:imu0: yourself_imu_topic

然后要修改launch/include/module_navsat.launch文件：

    <!-- Navsat --><node pkg="robot_localization" type="navsat_transform_node" name="navsat" respawn="true"><!-- <rosparam param="datum">[42.35893211, -71.09345588, 0.0, world, base_link]</rosparam> --><remap from="imu/data" to="yourself_imu_topic" /><remap from="gps/fix" to="yourself_gps_topic" /><remap from="odometry/filtered" to="odometry/navsat" /></node>

最后最重要的一步是要修改launch/include/config/robot.urdf.xacro文件，不然robot_localization功能包无法发布"odometry/gps"，修改此文件主要是添加imu->base_link，gps->base_link的tf变换，供robot_localization的节点查找并对IMU数据和GPS数据进行转换：

<?xml version="1.0"?>
<robot name="lio" xmlns:xacro="http://tixiaoshan.github.io/"><xacro:property name="PI" value="3.1415926535897931" /><link name="chassis_link"></link><link name="base_link"></link><joint name="base_link_joint" type="fixed"><parent link="base_link"/><child link="chassis_link" /><origin xyz="0 0 0" rpy="0 0 0" /></joint><link name="imu_link"> </link><joint name="imu_joint" type="fixed"><parent link="chassis_link" /><child link="imu_link" /><origin xyz="0 0 0" rpy="0 0 0" /></joint><link name="velodyne"> </link><joint name="velodyne_joint" type="fixed"><parent link="chassis_link" /><child link="velodyne" /><origin xyz="0 0 0" rpy="0 0 0" /></joint><link name="navsat_link"> </link><joint name="navsat_joint" type="fixed"><parent link="chassis_link" /><child link="navsat_link" /><origin xyz="0 0 0" rpy="0 0 0" /></joint><!-- 添加自己的传感器坐标系到base_link变换关系 --><link name="yourself_imu_Coordinate"> </link><joint name="yourself_imu_Coordinate_joint" type="fixed"><parent link="chassis_link" /><child link="yourself_imu_Coordinate" /><origin xyz="0 0 0" rpy="0 0 0" /></joint><link name="yourself_gps_Coordinate"> </link><joint name="yourself_gps_Coordinate_joint" type="fixed"><parent link="chassis_link" /><child link="yourself_gps_Coordinate" /><origin xyz="0 0 0" rpy="0 0 0" /></joint></robot>

修改完成以后即可运行lio-sam中Lidar + imu + gps的方案了，可能中间关于"imu_correct"理解不到位的地方，希望大家指出并一起交流学习，祝好！

融合视觉方案（LVI-SAM）

Lidar-Camera-IMU融合方案详细配置过程可以参考：LVI-SAM：配置环境、安装测试、适配自己采集数据集