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

前言

论文摘要

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

系统描述

依赖

• 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 

测试

1. Velodyne 32线激光雷达

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 ::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 的人不少，查了下邮箱，最近的是前天。不确定作者会不会再恢复，以及什么时候恢复。

新传感器

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.; 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的任何代码，请引用论文：