LeGO-LOAM初探：原理，安装和测试

前言

最近要搞3D激光SLAM，先后测试了Autoware，cartographer，loam和LeGO-LOAM。
今天就带来LeGO-LOAM的使用体验。
Github：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
论文：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/Shan_Englot_IROS_2018_Preprint.pdf
YouTube：https://www.youtube.com/watch?v=O3tz_ftHV48
以上视频展示了系统的关键模块，以及与LOAM在不同平台和场景上测试的结果，强烈建议观看。

论文摘要

我们提出了LeGO-LOAM，它是一种轻量级和地面优化的激光雷达里程计和建图方法，用于实时估计地面车辆的六自由度姿态。LeGO-LOAM是轻量级的，因为它可以在低功耗嵌入式系统上实现实时姿态估计。LeGO-LOAM经过地面优化，因为它在分割和优化步骤中利用了地面的约束。我们首先应用点云分割来滤除噪声，并进行特征提取，以获得独特的平面和边缘特征。然后，采用两步Levenberg-Marquardt优化方法，使用平面和边缘特征来解决连续扫描中六个自由度变换的不同分量。我们使用地面车辆从可变地形环境中收集的数据集，比较LeGO-LOAM与最先进的LOAM方法的性能，结果表明LeGO-LOAM在减少计算开销的情况下实现了相似或更好的精度。为了消除由漂移引起的姿态估计误差，我们还将LeGO-LOAM集成到SLAM框架中，并用KITTI数据集进行了测试。

系统描述

LeGO-LOAM针对地面车辆上水平放置的VLP-16进行了特别优化。它假设扫描中始终存在地平面。使用的UGV是Clearpath Jackal，它有一个内置的IMU。

在特征提取之前执行分割操作：

Lidar odometry执行两步Levenberg Marquardt优化以获得6D变换：

依赖

• ROS (tested with indigo and kinetic)
• gtsam (Georgia Tech Smoothing and Mapping library)

安装gtsam

1. 先决条件

• Boost >= 1.43 (Ubuntu: sudo apt-get install libboost-all-dev)
• CMake >= 2.6 (Ubuntu: sudo apt-get install cmake)
• A modern compiler, i.e., at least gcc 4.7.3 on Linux.

2. 下载gtsam

cd ~
git clone https://bitbucket.org/gtborg/gtsam.git

3. 编译

cd ~/gtsam
mkdir build
cd build
cmake ..
make check   #可选的，运行单元测试，我没执行这个命令，因为在TX2上编译太慢了，太慢了，太慢了
make install

注：在TX2编译gtsam较慢，请耐心等待。

下载并编译LeGO-LOAM

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
cd ..
catkin_make -j1

当第一次编译代码时，需要在“catkin_make”后面添加“-j1”以生成一些消息类型。将来的编译不需要“-j1”。

运行

1. 运行 launch 文件

roslaunch lego_loam run.launch

注意：参数“/ use_sim_time”，对于模拟则设置为“true”，对于使用真实机器人则设置为“false”。

2. 播放bag文件

rosbag play *.bag --clock --topic /velodyne_points /imu/data

注意：虽然 /imu/data 是可选的，但如果提供的话，它可以大大提高估计的准确性。
可从此处下载一些样例bag。
如果你的 IMU 帧与 Velodyne 帧不对齐，使用 IMU 数据将导致明显漂移。
如果对 ”–clock“ 有疑惑，可以参考：https://answers.ros.org/question/12577/when-should-i-need-clock-parameter-on-rosbag-play/ 。

测试

1. Velodyne 32线激光雷达

LeGO-LOAM能在NVIDIA Jetson TX2上达到实时的效果，而且建图效果非常好，尤其是Z轴没有明显的飘移。
部分截图如下：



2. RS-LIDAR-16

RS-LIDAR-16默认发布的 topic 为/rslidar_points，而LeGO-LOAM需要订阅的 topic 为/velodyne_points，因此使用以下命令：

rosbag play *.bag --clock /rslidar_points:=/velodyne_points

但是，出现以下错误：

featureAssociation: /build/pcl-2t3l2O/pcl-1.7.2/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector&, std::vector&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && “Invalid (NaN, Inf) point coordinates given to nearestKSearch!”’ failed.
[featureAssociation-6] process has died [pid 4388, exit code -6, cmd /home/nvidia/LeGO-LOAM-catkin_ws/devel/lib/lego_loam/featureAssociation __name:=featureAssociation __log:=/home/nvidia/.ros/log/20171d1e-1a4c-11e9-bf05-00044bc4e145/featureAssociation-6.log].
log file: /home/nvidia/.ros/log/20171d1e-1a4c-11e9-bf05-00044bc4e145/featureAssociation-6*.log

猜测是要针对RS-LIDAR-16的硬件特性，在代码上做一些必要的修改，因为LeGO-LOAM是专门基于VLP-16优化的。

参考下一部分【新传感器】的内容，如果RS-LIDAR-16与VLP-16硬件参数不同，需要修改参数，但是有点疑惑，因为在没有修改参数的前提下，Velodyne 32线激光雷达数据效果非常好。

本想在Github上提issue，结果没有了入口。代码刚开源时，我就 watch 了，平时提 issue 的人不少，查了下邮箱，最近的是前天。不确定作者会不会再恢复，以及什么时候恢复。

新传感器

使代码适应新传感器的关键是确保点云可以正确投影到距离图像，并且可以正确检测地面。例如，VLP-16沿两个方向的角分辨率为0.2°和2°。它有16个光束。最底部光束的角度为-15°。
因此，“utility.h” 中的参数如下所示。
使用新传感器时，请确保 ground_cloud 具有足够的匹配点。在发布任何问题之前，请阅读此内容。

extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;
extern const float ang_res_y = 2.0;
extern const float ang_bottom = 15.0;
extern const int groundScanInd = 7;

Velodyne HDL-32e:

extern const int N_SCAN = 32;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 360.0/Horizon_SCAN;
extern const float ang_res_y = 41.333/float(N_Scan-1);
extern const float ang_bottom = 30.666666;
extern const int groundScanInd = 20;

需要记住的一件重要事情是，我们目前的距离图像投影实现仅适用于具有均匀分布通道的传感器。如果你想将我们的算法与Velodyne VLP-32c或HDL-64e一起使用，你需要为此类投影编写自己的实现。如果点云未正确投射，您将失去许多点和性能。

如果您将激光雷达与IMU配合使用，请确保您的IMU与激光雷达正确对齐。该算法使用IMU数据来校正由传感器运动引起的点云失真。如果IMU未正确对齐，则IMU数据的使用将使结果恶化。软件包不支持Ouster激光雷达和IMU。

保存地图

在终端窗口，键入ctrl+c退出程序后，在/tmp目录下会生成4个pcd文件，分别为：

• finalCloud.pcd
• cornerMap.pcd
• surfaceMap.pcd
• trajectory.pcd

/tmp目录下的文件在每次重启后都会被清空，如果想保存在其他路径下，需要在mapOptmization.cpp中的visualizeGlobalMapThread函数中修改路径：

// save final point cloud
pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS);

string cornerMapString = "/tmp/cornerMap.pcd";
string surfaceMapString = "/tmp/surfaceMap.pcd";
string trajectoryString = "/tmp/trajectory.pcd";

特点：地图比较稀疏。

原因：对点云进行了下采样（dowmsample）

在终端执行以下命令，可以打印pose：

rostopic echo /aft_mapped_to_init

在终端执行以下命令，可以将pose保存到pose.txt中：

rostopic echo /aft_mapped_to_init > pose.txt

但是，这种方式不方便数据分析，需要以一定格式存储。

---

有网友给我私信和评论，说无法保存地图。

因此，我测试了一下，发现确实不能保存地图。

报错：

terminate called after throwing an instance of 'pcl::IOException' what(): : [pcl::PCDWriter::writeASCII] Input point cloud has no data! 

尝试在mapOptmization.cpp中的visualizeGlobalMapThread函数中修改：

if (globalMapKeyFramesDS->size() > 0) {
    // save final point cloud
    pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS);
}

即当globalMapKeyFramesDS的大小大于0时，才保存为PCD文件。这种方法可以生成除了finalCloud.pcd之外的PCD文件，没有从根本上解决问题。

继续检查publishGlobalMap函数，发现下面代码：

if (pubLaserCloudSurround.getNumSubscribers() == 0)
	return;

表明我们需要订阅pubLaserCloudSurround，否则会退出函数，不执行后面的代码，导致globalMapKeyFramesDS为空。

因此，当bag播放完毕后，在rviz中勾选Map Cloud，然后按ctrl+c退出程序，最终可以在/tmp目录下生成四个pcd文件。

这个问题是代码更新导致的，具体为https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/launch/test.rviz中Map Cloud由默认的勾选变成不勾选。

引用LeGO-LOAM

如果使用LeGO-LOAM的任何代码，请引用论文：

@inproceedings{legoloam2018,
title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain},
author={Tixiao Shan and Brendan Englot},
booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
pages={4758-4765},
year={2018},
organization={IEEE}
}

注：开源不易，如果对你有益，请支持作者的工作。
另外，watch，star，fork也是对作者的支持。