速腾16线激光雷达运行LOAM、A-LOAM和LeGO-LOAM

二维激光雷达SLAM已经相当成熟，前一段时间开始接触三维激光雷达，发现资料很少，遇到了很多问题，也有一些收获，趁着假期整理一下。本人使用turtlebot搭载速腾16线激光雷达（只使用激光雷达，没有用到IMU），运行并对比LOAM、A-LOAM和LeGO-LOAM异同。

环境：

1、速腾16线激光雷达rslidar；
2、工控机；
3、Ubuntu 16.04;
4、Turtlebot。

参考博客：
1.记录robosense16多线雷达配置过程（同门的博客）https://blog.csdn.net/Anubissz/article/details/107838647
2. 3D-SLAM 入门教程-多线雷达(RSlidar 16)loam_velodyne 3D 建图https://www.ncnynl.com/archives/201702/1382.html
3.保存并查看 Loam 的三维点云地图https://blog.csdn.net/qq_36396941/article/details/83048415
4. A-LOAM的github官方安装方法
https://github.com/HKUST-Aerial-Robotics/A-LOAM
5. Ceres安装报错“找不到要求版本3.3的Eigen3”解决方法https://www.cnblogs.com/didada/p/12305066.html
6. ceres报错：Eigen3版本和ceres版本冲突问题
https://zhuanlan.zhihu.com/p/149775218?from_voters_page=true
7. LOAM的编译安装运行（安装PCL1.8）
https://blog.csdn.net/weixin_43211438/article/details/87818526
8. LeGO-LOAM的github官方安装方法
https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
9. LeGO-LOAM初探：原理，安装和测试https://blog.csdn.net/learning_tortosie/article/details/86527542
10. GTSAM和LeGO-LOAM安装调试错误与解决方案
https://zhuanlan.zhihu.com/p/123446216
11. 【激光雷达】3D激光雷达传感器建图：速腾聚创、velodyne建图过程总结https://blog.csdn.net/xingdou520/article/details/85098314
12. LeGO-LOAM和LOAM的区别与联系
https://zhuanlan.zhihu.com/p/115986186

1. robosense16多线雷达配置

1.1 安装驱动源码

$ mkdir –p ~/catkin_rslidar/src
$ cd catkin_rslidar/src
$ git clone https://github.com/RoboSense-LiDAR/ros_rslidar

1.2 安装 libpcap-dev

$ sudo apt-get install libpcap-dev

1.3 更改源码属性

$ cd ros_rslidar/rslidar_drvier
$ chmod 777 cfg/*
$ cd ..
$ cd rslidar_pointcloud
$ chmod 777 cfg/*

1.4 catkin make

1.5 将自己的工控机与雷达连接，修改 ip

通过ifconfig 查看网口名字，enp4s0 是本人的工控机的网口名字。
将本地 ip 地址改为 192.168.1.102，子网掩码设置为 255.255.255.0：

$ sudo ifconfig enp4s0 192.168.1.102 netmask 255.255.255.0

1.6 测试

$ cd ~/catkin_rslidar
$ source devel/setup.bash
$ roslaunch rslidar_pointcloud rs_lidar_16.launch

看到多线雷达的画面：

2. LOAM 安装和测试

2.1 安装 loam_velodyne

$ cd catkin_rslidar/src
$ git clone https://github.com/laboshinl/loam_velodyne.git
$ cd ..
$ catkin_make

其中，我遇到了问题catkin_make 运行出错：

查到解决方案如下：

2.2 rslidar 跑 loam 的准备

进入 launch 目录

$ roscd loam_velodyne/launch/

增加 loam_rslidar.launch(内容见附录)，由于未使用imu，去掉了两句 imu 数据的映射。

<launch>
  <node pkg="tf" type="static_transform_publisher" name="rslidarlink_broadcaster" args="0.0 0.0 0.42 0.0 0.0 0.0 base_link laser 50"/>

  <arg name="rviz" default="false" />
  <arg name="scanPeriod" default="0.1" />
  <arg name="lidartype" default="VLP-16" /> <!-- options: VLP-16  HDL-32  HDL-64E RS-32 RS-16-->

  <node pkg="loam_velodyne" type="multiScanRegistration" name="multiScanRegistration" output="screen">
    <param name="lidar" value="$(arg lidartype)" /> <!-- options: VLP-16  HDL-32  HDL-64E RS-32 RS-16 -->
    <param name="scanPeriod" value="$(arg scanPeriod)" />
    <remap from="/multi_scan_points" to="/rslidar_points" />
    <!--remap from="/imu/data"  to="/mobile_base/sensors/imu_data" /-->
  </node>

  <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
    <param name="scanPeriod" value="$(arg scanPeriod)" />
  </node>

  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen">
    <param name="scanPeriod" value="$(arg scanPeriod)" />
    <!--remap from="/imu/data"  to="/mobile_base/sensors/imu_data" /-->
  </node>

  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen">
  </node>

  <group if="$(arg rviz)">
    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />
  </group>
</launch>

2.3 LOAM实际运行过程

loam_velodyne 实时建图：
修改雷达 IP：

$ sudo ifconfig enp4s0 192.168.1.102 netmask 255.255.255.0

启动 rslidar 雷达

$ roslaunch rslidar_pointcloud rs_lidar_16.launch

新终端，启动 loam_velodyne 建图和 Rviz

$ roslaunch loam_velodyne loam_rslidar.launch

启动底盘

$ roslaunch turtlebot_bringup minimal.launch

启动键盘控制建图

$ roslaunch turtlebot_teleop keyboard_teleop.launch

建图效果如下：

2.4 保存并查看 loam 的三维点云地图

在建图过程中执行， 来录制 loam 后生成的地图。点云话题是 laser_cloud_surround，
录好之后 ctrl+c 结束， mybag.bag 是录好之后的包的名字。

$ rosbag record -o mybag.bag out /laser_cloud_surround

将 bag 文件转换成 pcd 文件：

$ rosrun pcl_ros bag_to_pcd mybag.bag /laser_cloud_surround mypcd

mypcd 是转换成 pcd 的文件夹名字。获取的 pcd 文件是很多个，每一个 pcd 是一
帧数据。可以通过 pcl_viewer 命令查看 pcd 格式的最后一个文件：

$ pcl_viewer yourname.pcd

loam是三维slam中最基础的算法。实际运行过程中，小车转弯时，构建的地图容易翻转，效果不是很好。

3. A-LOAM 安装和测试

3.1 Ceres Solver

github上安装A-LOAM步骤：（These instructions are for Ubuntu 18.04 and newer. On Ubuntu 16.04 you need to manually get a more recent version of Eigen, such as 3.3.7.）
https://github.com/HKUST-Aerial-Robotics/A-LOAM
注释中强调他的步骤是针对Ubuntu 18.04及更新版本，由于我的Ubuntu是16.04。按照此步骤进行，安装Eigen3.3报错：Ceres安装报错“找不到要求版本3.3的Eigen3”，查找解决方案，可直接安装Eigen3.3.7：（或遇到此错误按照此方法卸载重装3.3.7）
https://www.cnblogs.com/didada/p/12305066.html
按步骤安装了ceres2.0.0，ceres报错：Eigen3版本和ceres版本冲突问题，查找解决方案，直接安装ceres1.14.0（或遇到此错误按照此方法卸载重装1.14.0）
https://zhuanlan.zhihu.com/p/149775218?from_voters_page=true

3.2 PCL

github步骤中pcl点击失效，如若PCL1.7不能运行，可卸载PCL1.7，安装PCL1.8。（我用第二台工控机安装的时候，直接就可以运行）
安装PCL1.8可参考：https://blog.csdn.net/weixin_43211438/article/details/87818526

3.3 A-LOAM实际运行

$ roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch

运行此指令便可看到效果，但是我的却报错，如遇到此情况

找不到两个包，用直接运行cpp方式运行其余两个节点：

$ cd turbot_ws/devel/lib/aloam_velodyne
$ ./alaserMapping
$ ./alaserOdometry

保存并查看 A-LOAM 的三维点云地图，点云相对稠密：

A-LOAM可以生成robot运动轨迹：

$ rostopic echo /aft_mapped_path

将位姿输出成aloam_pose.txt：

$ rostopic echo /aft_mapped_path > aloam_pose.txt

轨迹在一个平面上。

4. LeGO-LOAM 安装和测试

4.1 安装gtsam并编译

$ git clone https://bitbucket.org/gtborg/gtsam.git

编译

$ cd ~/gtsam
$ mkdir build
$ cd build
$ cmake ..
$ sudo make install(一定要加上sudo)

4.2 下载并编译LeGO-LOAM

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

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

解决方案：
将/usr/local/lib/cmake/GTSAM/GTSAMConfig.cmake:17 行的find_dependency改成find_package。具体操作：

$ cd /usr/local/lib/cmake/GTSAM/
$ sudo chmod a+w GTSAMConfig.cmake
$ gedit GTSAMConfig.cmake

重新运行

$ cd ~/lego_loam_test
$ catkin_make -j1

4.3 数据集运行

运行launch文件:（这个rviz界面确实是黑的，不要慌张）

$ roslaunch lego_loam run.launch

播放bag文件（数据集）

$ rosbag play *.bag --clock --topic /velodyne_points

4.4 LeGO-LOAM实际运行

修改雷达发布的话题，convert.cc中output_points_topic由rslidar_points改为velodyne_points（rslidar16默认发布的 topic 为/rslidar_points，而LeGO-LOAM需要订阅的 topic 为/velodyne_points,这个问题耽误进度很久）。再改“utility.h”中的参数，即可。开启雷达后，运行lego-loam：

$ roslaunch lego_loam run.launch

生成4个pcd文件：finalCloud.pcd，cornerMap.pcd，surfaceMap.pcd，trajectory.pcd。最终点云地图，点云相对少。

录制自己的数据包：

$ rosbag record -O mybag.bag /velodyne_points

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

$ rostopic echo /aft_mapped_to_init

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

$ rostopic echo /aft_mapped_to_init > pose.txt

5. 总结、对比：

图片均来自博客：https://zhuanlan.zhihu.com/p/115986186。

LOAM进行了特征点提取和匹配，边缘点和平面点。
缺点：不能处理大规模的旋转变换。

A-LOAM是LOAM简化版本。
A-LOAM没有提供IMU信息修正的接口，缺少对提取到的特征点的筛选；
LOAM使用LM优化方法，A-LOAM则是使用了Ceres库完成；
LOAM中解析地求出了雅可比的表达式，A-LOAM使用了Ceres提供的自动求导工具。

LeGO-LOAM特点：轻量级，点云分割去除噪声，带有地面优化的实时6自由度估计，回环检测。
增加了更多预处理步骤，包括地面的提取，和点云分割。使用更多筛选之后的点云再提取特征点；
在提取特征点时，将点云分成小块，分别提取特征点，以保证特征点的均匀分布；
一个双步骤的LM优化，先使用平面点优化高度，同时利用地面的方向优化两个角度信息；再使用边角点优化剩下的三个变量；
不用的地图点存储方式。LOAM中将所有历史的点云存储到同一张点云图中。LeGO-LOAM则是分别存储每一帧的特征点信息和每一帧的位姿数据；
适合室外场景。
原理和代码还得认真学习和钻研，加油！如有错误，欢迎纠正。