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。
LeGO-LOAM初探:原理,安装和测试_第1张图片
在特征提取之前执行分割操作:
LeGO-LOAM初探:原理,安装和测试_第2张图片
Lidar odometry执行两步Levenberg Marquardt优化以获得6D变换:
LeGO-LOAM初探:原理,安装和测试_第3张图片

依赖

  • 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.
  1. 下载gtsam
cd ~
git clone https://bitbucket.org/gtborg/gtsam.git
  1. 编译
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”。

  1. 播放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轴没有明显的飘移。
部分截图如下:
LeGO-LOAM初探:原理,安装和测试_第4张图片
LeGO-LOAM初探:原理,安装和测试_第5张图片
LeGO-LOAM初探:原理,安装和测试_第6张图片
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::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

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

引用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也是对作者的支持。

你可能感兴趣的:(SLAM)