激光SLAM：Faster-Lio算法編譯與測試

來源：古月居

前言

Faster-LIO是基于FastLIO2開發(fā)的。FastLIO2是開源LIO中比較優(yōu)秀的一個，前端用了增量的kdtree（ikd-tree），后端用了迭代ESKF（IEKF），流程短，計算快。Faster-LIO則把ikd-tree替換成了iVox，順帶優(yōu)化了一些代碼邏輯，實現(xiàn)了更快的LIO。在典型的32線激光雷達中可以取得100-200Hz左右的計算頻率，在固態(tài)雷達中甚至可以達到1000-2000Hz，能夠達到FastLIO2的1.5-2倍左右的速度。當然具體數值和計算平臺相關。

FasterLIO使用了一種基于稀疏體素的近鄰結構iVox（incremental voxels）。我們會發(fā)現(xiàn)這種結構用來做LIO更加合適，可以有效的降低點云配準時的耗時，也不會影響LIO的精度表現(xiàn)。

iVox也可以被集成到其他LO或LIO里，但是大部分方案里，最近鄰并不是主要的計算瓶頸，gtsam/ceres什么的耗時相比最近鄰那可太多了。、把iVox集成到Lego-LOAM里，、主要只是省了增量地圖構建那部分時間，優(yōu)化方面沒什么變化（點少）。所以iVox與FastLIO倒是相性更好一些。

編譯

部署系統(tǒng)：ubuntu20.04
ROS版本: noetic

github 地址：https://github.com/gaoxiang12/faster-lio

下載源碼

git clone https://github.com/gaoxiang12/faster-lio

正克隆到 ‘faster-lio’…
remote: Enumerating objects: 224, done.
remote: Counting objects: 100% (108/108), done.
remote: Compressing objects: 100% (43/43), done.
remote: Total 224 (delta 76), reused 65 (delta 65), pack-reused 116
接收對象中: 100% (224/224), 38.13 MiB | 1.49 MiB/s, 完成.
處理 delta 中: 100% (97/97), 完成.

將原文件拷入ros工作空間

依賴

?ROS (melodic or noetic)

?glog: sudo apt-get install libgoogle-glog-dev

?eigen: sudo apt-get install libeigen3-dev

?pcl: sudo apt-get install libpcl-dev

?yaml-cpp: sudo apt-get install libyaml-cpp-dev

編譯

catkin_make

報錯1：

CMake Error at /home/jk-jone/jone_ws/build/livox_ros_driver/livox_ros_driver/cmake/livox_ros_driver-genmsg.cmake:14 (add_custom_target):
add_custom_target cannot create target “l(fā)ivox_ros_driver_generate_messages”
because another target with the same name already exists. The existing
target is a custom target created in source directory
“/home/jk-jone/jone_ws/src/faster-lio/thirdparty/livox_ros_driver”. See
documentation for policy CMP0002 for more details.
Call Stack (most recent call first):
/opt/ros/noetic/share/genmsg/cmake/genmsg-extras.cmake:307 (include)
livox_ros_driver/livox_ros_driver/CMakeLists.txt:46 (generate_messages)

如果工作空間中之前編譯了 livox_ros_driver 的功能包，則需要刪掉 faster-lio/thirdparty/livox_ros_driver 這個文件夾

再次編譯

CMake Error at faster-lio/CMakeLists.txt:15 (add_subdirectory):
add_subdirectory given source “thirdparty/livox_ros_driver” which is not an
existing directory.

因為把那個文件刪了，所以找不到路徑

將 faster-lio/CMakeLists.txt 文件的第15行注釋掉

add_subdirectory(thirdparty/livox_ros_driver)
改為
#add_subdirectory(thirdparty/livox_ros_driver)

再次編譯

[100%] Linking CXX shared library /home/jk-jone/jone_ws/devel/lib/libfaster_lio.so
[100%] Built target faster_lio
Scanning dependencies of target run_mapping_offline
Scanning dependencies of target run_mapping_online
[100%] Building CXX object faster-lio/app/CMakeFiles/run_mapping_online.dir/run_mapping_online.cc.o
[100%] Building CXX object faster-lio/app/CMakeFiles/run_mapping_offline.dir/run_mapping_offline.cc.o
[100%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/faster_lio/run_mapping_online
[100%] Built target run_mapping_online
[100%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/faster_lio/run_mapping_offline
[100%] Built target run_mapping_offline

編譯成功

測試

Faster-lio支持離線的測試與在線測試

離線測試

首先下載rosbag數據包到電腦

aviabags(https://drive.google.com/drive/folders/1YL5MQVYgAM8oAWUm7e3OGXZBPKkanmY1?usp=sharing)

ncltbags(https://drive.google.com/drive/folders/1VBK5idI1oyW0GC_I_Hxh63aqam3nocNK)

百度云盤下載地址：
BaiduYun:https://pan.baidu.com/s/1ELOcF1UTKdfiKBAaXnE8sQ?pwd=fekyaccess code: feky

OneDrive下載地址：
OneDrive：https://1drv.ms/u/s!AgNFVSzSYXMahcEZejoUwCaHRcactQ?e=YsOYy2

Call run_mapping_offline with parameters to specify the bag file and the config file like:
通過下面的指令 運行 run_mapping_offline 文件 并且加載對應的rosbag文件 和對應的配置文件

./build/devel/lib/faster_lio/run_mapping_offline --bag_file your_avia_bag_file --config_file ./config/avia.yaml

其中 your_avia_bag_file 路徑需要更換為下載的數據包路徑

同樣對于nclt數據包可以運行下面的指令。數據是機械式激光雷達velodyne的數據

./build/devel/lib/faster_lio/run_mapping_offline --bag_file your_nclt_bag_file --config_file ./config/velodyne.yaml

your_nclt_bag_file 路徑需要更換為下載的數據包路徑

運行FasterLIO，然后退出的時候 會在終端打印FPS和time

像下面這樣：

I0216 17:16:05.286536 26492 run_mapping_offline.cc:89] Faster LIO average FPS: 1884.6
I0216 17:16:05.286549 26492 run_mapping_offline.cc:91] save trajectory to: ./src/fast_lio2/Log/faster_lio/20120615.tum
I0216 17:16:05.286706 26492 utils.h:52] >>> ===== Printing run time =====
I0216 17:16:05.286711 26492 utils.h:54] > [   IVox Add Points ] average time usage: 0.0147311 ms , called times: 6373
I0216 17:16:05.286721 26492 utils.h:54] > [   Incremental Mapping ] average time usage: 0.0271787 ms , called times: 6373
I0216 17:16:05.286731 26492 utils.h:54] > [   ObsModel (IEKF Build Jacobian) ] average time usage: 0.00745852 ms , called times: 25040
I0216 17:16:05.286752 26492 utils.h:54] > [   ObsModel (Lidar Match) ] average time usage: 0.0298004 ms , called times: 25040
I0216 17:16:05.286775 26492 utils.h:54] > [ Downsample PointCloud ] average time usage: 0.0224052 ms , called times: 6373
I0216 17:16:05.286784 26492 utils.h:54] > [ IEKF Solve and Update ] average time usage: 0.342008 ms , called times: 6373
I0216 17:16:05.286792 26492 utils.h:54] > [ Laser Mapping Single Run ] average time usage: 0.530618 ms , called times: 6387
I0216 17:16:05.286800 26492 utils.h:54] > [ Preprocess (Livox) ] average time usage: 0.0267813 ms , called times: 6387
I0216 17:16:05.286808 26492 utils.h:54] > [ Undistort Pcl ] average time usage: 0.0810455 ms , called times: 6375
I0216 17:16:05.286816 26492 utils.h:59] >>> ===== Printing run time end =====

默認點云會以pcd文件的格式保存下來

在線測試

用之前建立的仿真環(huán)境下的 mid360雷達的數據進行一個初步在線測試

打開仿真環(huán)境

faster-lio 里面沒有 mid360 雷達的 配置文件和啟動文件 ，有avia的，都是livox的固態(tài)雷達，基本雷達，仿照avia的寫一個就行

mid360.yaml 如下

common:
  lid_topic: "/livox/lidar"
  imu_topic: "/livox/imu"
  time_sync_en: false     # ONLY turn on when external time synchronization is really not possible
  time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
                 # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0


preprocess:
  lidar_type: 1        # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
  scan_line: 4
  blind: 0.5


mapping:
  acc_cov: 0.1
  gyr_cov: 0.1
  b_acc_cov: 0.0001
  b_gyr_cov: 0.0001
  fov_degree:  360
  det_range:   100.0
  extrinsic_est_en: false   # true: enable the online estimation of IMU-LiDAR extrinsic
  extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
  extrinsic_R: [ 1, 0, 0,
          0, 1, 0,
          0, 0, 1]


publish:
  path_en: false
  scan_publish_en: true    # false: close all the point cloud output
  dense_publish_en: true    # false: low down the points number in a global-frame point clouds scan.
  scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame


pcd_save:
  pcd_save_en: true
  interval: -1         # how many LiDAR frames saved in each pcd file; 
                 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

launch文件如下

運行該launch文件

roslaunch faster_lio mapping_mid360.launch

初始位置的情景和點云模型

飛一圈后整個地圖模型

審核編輯：湯梓紅