# LIO-SAM
**一个实时的 lidar-inertial odometry 软件包。我们强烈建议用户通读本文档，并先使用提供的数据集测试该软件包。关于该方法的演示视频可以在 [YouTube](https://www.youtube.com/watch?v=A0H8CoORZJU) 上找到。**
## 菜单
- [**系统架构**](#system-architecture)
- [**软件包依赖**](#dependency)
- [**软件包安装**](#install)
- [**准备 lidar 数据**](#prepare-lidar-data) (必读)
- [**准备 IMU 数据**](#prepare-imu-data) (必读)
- [**示例数据集**](#sample-datasets)
- [**运行软件包**](#run-the-package)
- [**其他说明**](#other-notes)
- [**问题**](#issues)
- [**论文**](#paper)
- [**待办事项**](#todo)
- [**相关软件包**](#related-package)
- [**致谢**](#acknowledgement)
## 系统架构
我们设计了一个维护两个图的系统，其运行速度比实时快达 10 倍。
- "mapOptimization.cpp" 中的因子图优化 lidar odometry 因子和 GPS 因子。该因子图在整个测试过程中持续维护。
- "imuPreintegration.cpp" 中的因子图优化 IMU 和 lidar odometry 因子并估计 IMU bias。该因子图会周期性重置，并保证以 IMU 频率进行实时 odometry 估计。
## 依赖
这是 LIO-SAM 的原始 ROS1 实现。关于 ROS2 实现，请参阅分支 `ros2`。
- [ROS](http://wiki.ros.org/ROS/Installation) (已在 Kinetic 和 Melodic 上测试。关于 Noetic 请参考 [#206](https://github.com/TixiaoShan/LIO-SAM/issues/206))
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
- [gtsam](https://gtsam.org/get_started/) (Georgia Tech Smoothing and Mapping library)
sudo add-apt-repository ppa:borglab/gtsam-release-4.0
sudo apt install libgtsam-dev libgtsam-unstable-dev
## 安装
使用以下命令下载并编译该软件包。
```
cd ~/catkin_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd ..
catkin_make
```
## 使用 Docker
构建镜像 (基于 ROS1 Kinetic):
```
docker build -t liosam-kinetic-xenial .
```
拥有镜像后，按如下方式启动容器：
```
docker run --init -it -d \
-v /etc/localtime:/etc/localtime:ro \
-v /etc/timezone:/etc/timezone:ro \
-v /tmp/.X11-unix:/tmp/.X11-unix \
-e DISPLAY=$DISPLAY \
liosam-kinetic-xenial \
bash
```
## 准备 lidar 数据
用户需要准备正确格式的点云数据以进行点云去畸变，这主要在 "imageProjection.cpp" 中完成。两个要求是：
- **提供点时间戳**。LIO-SAM 使用 IMU 数据执行点云去畸变。因此，需要知道扫描中点的相对时间。最新的 Velodyne ROS 驱动程序应直接输出此信息。这里，我们假设点时间通道称为 "time"。点类型的定义位于 "imageProjection.cpp" 的顶部。"deskewPoint()" 函数利用此相对时间来获取该点相对于扫描开始的变换。当 lidar 以 10Hz 旋转时，点的时间戳应在 0 到 0.1 秒之间变化。如果您使用的是其他 lidar 传感器，可能需要更改此时间通道的名称，并确保它是扫描中的相对时间。
- **提供点环号 (ring number)**。LIO-SAM 使用此信息将点正确组织在矩阵中。环号指示该点属于传感器的哪个通道。点类型的定义位于 "imageProjection.cpp" 的顶部。最新的 Velodyne ROS 驱动程序应直接输出此信息。同样，如果您使用的是其他 lidar 传感器，可能需要重命名此信息。请注意，该软件包目前仅支持机械式 lidar。
## 准备 IMU 数据
- **IMU 要求**。与原始 LOAM 实现一样，LIO-SAM 仅适用于 9 轴 IMU，它提供 roll、pitch 和 yaw 估计。Roll 和 pitch 估计主要用于以正确的姿态初始化系统。当使用 GPS 数据时，yaw 估计用于以正确的航向初始化系统。理论上，像 VINS-Mono 这样的初始化过程将使 LIO-SAM 能够与 6 轴 IMU 一起工作。(**新**: [liorf](https://github.com/YJZLuckyBoy/liorf) 已添加对 6 轴 IMU 的支持。) 系统的性能很大程度上取决于 IMU 测量的质量。IMU 数据速率越高，系统精度越好。我们使用 Microstrain 3DM-GX5-25，它以 500Hz 输出数据。我们建议使用输出速率至少为 200Hz 的 IMU。请注意，Ouster lidar 的内部 IMU 是 6 轴 IMU。
- **IMU 对齐**。LIO-SAM 将 IMU 原始数据从 IMU 坐标系转换到 Lidar 坐标系，这遵循 ROS REP-105 约定 (x - 前, y - 左, z - 上)。为了使系统正常工作，需要在 "params.yaml" 文件中提供正确的外参变换。**之所以有两个外参是因为我的 IMU (Microstrain 3DM-GX5-25) 的加速度和姿态具有不同的坐标系。根据您的 IMU 制造商，您的 IMU 的这两个外参可能相同，也可能不同**。以我们的设置为例：
- 我们需要将 x-z 加速度和陀螺仪的读数设置为负值，以将 IMU 数据转换到 lidar 坐标系，这在 "params.yaml" 中由 "extrinsicRot" 指示。
- 姿态读数的变换可能略有不同。IMU 的姿态测量 `q_wb` 通常意味着将 IMU 坐标系中的点旋转到世界坐标系 (例如 ENU)。然而，算法需要 `q_wl`，即从 lidar 到世界的旋转。所以我们需要一个从 lidar 到 IMU 的旋转 `q_bl`，其中 `q_wl = q_wb * q_bl`。为方便起见，用户只需在 "params.yaml" 中提供 `q_lb` 作为 "extrinsicRPY" (如果加速度和姿态具有相同的坐标系，则与 "extrinsicRot" 相同)。
- **IMU 调试**。强烈建议用户取消注释 "imageProjection.cpp" 中 "imuHandler()" 的调试行，并测试转换后的 IMU 数据的输出。用户可以旋转传感器套件以检查读数是否与传感器的运动相对应。可以在 [此处 (YouTube 链接)](https://youtu.be/BOUK8LYQhHs) 找到显示正确 IMU 数据的 YouTube 视频。
## 示例数据集
* 下载一些示例数据集以测试软件包的功能。以下数据集配置为使用默认设置运行：
- **Walking 数据集:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
- **Park 数据集:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
- **Garden 数据集:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
* 以下数据集需要配置参数。在这些数据集中，点云 topic 是 "points_raw"。IMU topic 是 "imu_correct"，它提供 ROS REP105 标准的 IMU 数据。由于此数据集不需要 IMU 变换，因此需要更改以下配置才能成功运行此数据集：
- "config/params.yaml" 中的 "imuTopic" 参数需要设置为 "imu_correct"。
- "config/params.yaml" 中的 "extrinsicRot" 和 "extrinsicRPY" 需要设置为单位矩阵。
- **Rotation 数据集:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
- **Campus 数据集 (大):** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
- **Campus 数据集 (小):** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
* Ouster (OS1-128) 数据集。如果您使用的是默认设置，则此数据集无需更改外参。请按照下面的 Ouster 说明配置软件包以运行 Ouster 数据。可以在 [YouTube](https://youtu.be/O7fKgZQzkEo) 上找到该数据集的视频：
- **Rooftop 数据集:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
* Livox Horizon 数据集。有关参数更改，请参阅以下说明部分。
- **Livox Horizon:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
* KITTI 数据集。外参可以在下面的 Notes KITTI 部分找到。要使用其他 KITTI 原始数据生成更多 bag，可以使用 "config/doc/kitti2bag" 中提供的 python 脚本。
- **2011_09_30_drive_0028:** [[Google Drive](https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq?usp=sharing)]
## 运行软件包
1. 运行 launch 文件：
```
roslaunch lio_sam run.launch
```
2. 播放现有的 bag 文件：
```
rosbag play your-bag.bag -r 3
```
## 其他说明
- **回环检测 (Loop closure):** 此处的回环功能提供了一个概念验证示例。它直接改编自 LeGO-LOAM 回环检测。关于更高级的回环检测实现，请参考 [ScanContext](https://github.com/irapkaist/SC-LeGO-LOAM)。将 "params.yaml" 中的 "loopClosureEnableFlag" 设置为 "true" 以测试回环检测功能。在 Rviz 中，取消选中 "Map (cloud)" 并选中 "Map (global)"。这是因为可视化的地图 - "Map (cloud)" - 只是 Rviz 中点云的简单堆叠。它们的位置在位姿校正后不会更新。此处的回环检测功能简单地改编自 LeGO-LOAM，这是一种基于 ICP 的方法。因为 ICP 运行相当慢，建议回放速度设置为 "-r 1"。您可以尝试使用 Garden 数据集进行测试。
- **使用 GPS:** 提供了 park 数据集用于测试带 GPS 数据的 LIO-SAM。该数据集由 [Yewei Huang](https://robustfieldautonomylab.github.io/people.html) 收集。要启用 GPS 功能，请将 "params.yaml" 中的 "gpsTopic" 更改为 "odometry/gps"。在 Rviz 中，取消选中 "Map (cloud)" 并选中 "Map (global)"。同时选中 "Odom GPS"，它会可视化 GPS odometry。可以调整 "gpsCovThreshold" 以过滤糟糕的 GPS 读数。"poseCovThreshold" 可用于调整向图中添加 GPS 因子的频率。例如，当您将 "poseCovThreshold" 设置为 1.0 时，您会注意到轨迹不断被 GPS 校正。由于繁重的 iSAM 优化，建议回放速度为 "-r 1"。
- **KITTI:** 由于 LIO-SAM 需要高频 IMU 才能正常工作，我们需要使用 KITTI 原始数据进行测试。一个尚未解决的问题是目前未知 IMU 的内参，这对 LIO-SAM 的准确性有很大影响。下载提供的示例数据并在 "params.yaml" 中进行以下更改：
- extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01]
- extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
- extrinsicRPY: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
- N_SCAN: 64
- downsampleRate: 2 or 4
- loopClosureEnableFlag: true or false
- **Ouster lidar:** 为了使 LIO-SAM 与 Ouster lidar 一起工作，需要在硬件和软件层面进行一些准备。
- 硬件：
- 使用外部 IMU。LIO-SAM 不适用于 Ouster lidar 的内部 6 轴 IMU。您需要将 9 轴 IMU 连接到 lidar 并进行数据收集。
- 配置驱动程序。将 Ouster launch 文件中的 "timestamp_mode" 更改为 "TIME_FROM_PTP_1588"，以便为点云提供 ROS 格式的时间戳。
- 配置：
- 将 "params.yaml" 中的 "sensor" 更改为 "ouster"。
- 根据 lidar 更改 "params.yaml" 中的 "N_SCAN" 和 "Horizon_SCAN"，即 N_SCAN=128, Horizon_SCAN=1024。
- Gen 1 和 Gen 2 Ouster:
似乎不同代产品的点坐标定义可能不同。请参考 [Issue #94](https://github.com/TixiaoShan/LIO-SAM/issues/94) 进行调试。
- **Livox Horizon lidar:** 请注意，固态 lidar 尚未在 LIO-SAM 中进行广泛测试。也使用外部 IMU 而不是内部 IMU。对此类 lidar 的支持是基于对机械式 lidar 代码库的最小更改。需要使用定制的 [livox_ros_driver](https://github.com/TixiaoShan/livox_ros_driver) 来发布 LIO-SAM 可以处理的点云格式。其他 SLAM 解决方案可能提供更好的实现。欢迎更多的研究和建议。请更改以下参数以使 LIO-SAM 与 Livox Horizon lidar 一起工作：
- sensor: livox
- N_SCAN: 6
- Horizon_SCAN: 4000
- edgeFeatureMinValidNum: 1
- 使用 [livox_ros_driver](https://github.com/TixiaoShan/livox_ros_driver) 进行数据记录
## 服务
- /lio_sam/save_map
- 将地图保存为 PCD 文件。
rosservice call [service] [resolution] [destination]
- 示例：
$ rosservice call /lio_sam/save_map 0.2 "/Downloads/LOAM/"
## 问题
- **锯齿或抖动行为**: 如果您的 lidar 和 IMU 数据格式符合 LIO-SAM 的要求，此问题很可能是由于 lidar 和 IMU 数据的时间戳不同步造成的。
- **上下跳动**: 如果您开始测试 bag 文件并且 base_link 立即开始上下跳动，很可能是您的 IMU 外参错误。例如，重力加速度值为负。
- **mapOptimization 崩溃**: 通常由 GTSAM 引起。请安装 README.md 中指定的 GTSAM。可以在 [此处](https://github.com/TixiaoShan/LIO-SAM/issues) 找到更多类似问题。
- **gps odometry 不可用**: 通常是由于 message frame_ids 和 robot frame_id 之间的变换不可用造成的 (例如：应该存在从 "imu_frame_id" 和 "gps_frame_id" 到 "base_link" frame 的变换。请阅读 [此处](http://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html) 找到的 Robot Localization 文档。
## 论文
如果您使用了任何此代码，请引用 [LIO-SAM (IROS-2020)](./config/doc/paper.pdf)。
```
@inproceedings{liosam2020shan,
title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela},
booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
pages={5135-5142},
year={2020},
organization={IEEE}
}
```
部分代码改编自 [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM)。
```
@inproceedings{legoloam2018shan,
title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain},
author={Shan, Tixiao and Englot, Brendan},
booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
pages={4758-4765},
year={2018},
organization={IEEE}
}
```
## 待办事项
- [ ] [imuPreintegration 中的 Bug](https://github.com/TixiaoShan/LIO-SAM/issues/104)
## 相关软件包
- [liorf](https://github.com/YJZLuckyBoy/liorf) 支持 6 轴 IMU 和更多 lidar 的 LIO-SAM。
- [Lidar-IMU 标定](https://github.com/chennuo0125-HIT/lidar_imu_calib)
- [带有 Scan Context 的 LIO-SAM](https://github.com/gisbi-kim/SC-LIO-SAM)
- [支持 6 轴 IMU 的 LIO-SAM](https://github.com/JokerJohn/LIO_SAM_6AXIS)
## 致谢
- LIO-SAM 基于 LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time)。