速騰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則是分別存盤每一幀的特征點資訊和每一幀的位姿資料；
適合室外場景，
原理和代碼還得認真學習和鉆研，加油！如有錯誤，歡迎糾正，