最近要搞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变换:
sudo apt-get install libboost-all-dev
)sudo apt-get install cmake
)cd ~
git clone https://bitbucket.org/gtborg/gtsam.git
cd ~/gtsam
mkdir build
cd build
cmake ..
make check #可选的,运行单元测试,我没执行这个命令,因为在TX2上编译太慢了,太慢了,太慢了
make install
注:在TX2编译gtsam较慢,请耐心等待。
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”。
roslaunch lego_loam run.launch
注意:参数“/ use_sim_time”,对于模拟则设置为“true”,对于使用真实机器人则设置为“false”。
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/ 。
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
::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文件,分别为:
/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的任何代码,请引用论文:
@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也是对作者的支持。