激光惯性 SLAM FAST-LIO2

FAST-LIO2 简介

论文地址：https://arxiv.org/abs/2107.06829

源码地址：https://github.com/hku-mars/FAST_LIO

如无法下载，可以尝试 gitee 版：https://gitee.com/sun_6s/FAST_LIO

FAST-LIO2：一种快速、稳健且通用的 LiDAR 惯性里程计框架。 FASTLIO2 建立在高效紧

耦合迭代卡尔曼滤波器的基础上，具有两个关键创新，可实现快速、稳健和准确的 LiDAR 导

航（和建图）。

FAST-LIO2 安装编译

cd ~/$FAST_LIO_ROS_DIR/src$

//首先安装 livox 的 sdk：

git clone https://github.com/Livox-SDK/Livox-SDK

cd Livox-SDK

mkdir build

cd build

cmake ..

make -j8

sudo make install

#随后安装 livox 的 ros 驱动：

cd src

git clone https://github.com/Livox-SDK/livox_ros_driver

git clone https://github.com/hku-mars/FAST_LIO

#注意，驱动和 fastlio 要放在同一个工作空间下。

#git submodule update --init 大概率是会失败的,直接 git clone

https://github.com/hku-mars/ikd-Tree 即可。

cd ~/$FAST_LIO_ROS_DIR$

catkin_make -j1

source devel/setup.bash

roslaunch fast_lio mapping_avia.launch

roslaunch livox_ros_driver livox_lidar_msg.launch

数据集运行

#运行官方数据集

roslaunch fast_lio mapping_avia.launch

rosbag play outdoor_Mainbuilding_100Hz_2020-12-24-16-46-29.bag

#运行 M2DGR 数据集(https://github.com/SJTU-ViSYS/M2DGR)：

cvlife.net 打开/fastlio_ws/src/FAST_LIO/config/velodyne.yaml

imu_topic 修改为"/handsfree/imu"

scan_line 改为 32

运行：~/fastlio_ws$ roslaunch fast_lio mapping_velodyne.launch

#打开数据集路径下的终端并运行

rosbag play street_03.bag

#记录轨迹并评估

rostopic echo /Odometry >> street3.txt

新建一个 py 文件，名为 topic2tum.py。将下列的 basedir 和 txtdir 分别改为 street3.txt 所在

路径与你的目标路径。运行 python topic2tum.py,即可得到 tum 格式的轨迹结果。

使用 evo_ape tum gt.txt street_03.txt -vap 即可得到评估结果

# -*- coding: utf-8 -*-
import numpy as np
import os, sys,re
name="street3"
basedir="/xxxxx"+name+'.txt'
txtdir="/xxxxx/"+name+'.txt'
f1=open(basedir,'r')
f2=open(txtdir,"w")
#f2.write('#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-
1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z
[m s^-2]\n')
line=f1.readline()
pflag=0
oflag=0
while line:
if 'secs' in line and 'nsecs' not in line:
sec=int(re.findall(r"\-?\d+\.?\d*" ,line)[0])
if 'nsecs' in line:
nsec=int(re.findall(r"\-?\d+\.?\d*" ,line)[0])
#f2.write(str(sec+nsec*0.000000001)+' ')
f2.write(str('%.9f' %(sec+nsec*0.000000001))+' ')
if 'position:' in line:
pflag=1
cvlife.net oflag=0
if 'orientation:' in line:
pflag=0
oflag=1
if 'x:' in line and pflag:
px=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
#f2.write(str(px)+' ')
if 'y:' in line and pflag:
py=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
#f2.write(str(py)+' ')
if 'z:' in line and pflag:
pz=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
#f2.write(str(pz)+' ')
if 'w:' in line and oflag:
ww=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
f2.write(str(px)+' '+str(py)+' 0 '+str(ww)+' '+str(wx)+'
'+str(wy)+' '+str(wz)+'\n')
if 'x:' in line and oflag:
wx=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
# f2.write(str(wx)+' ')
if 'y:' in line and oflag:
wy=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
# f2.write(str(wy)+' ')
if 'z:' in line and oflag:
wz=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
#f2.write(str(wz)+'\n')

line=f1.readline()

工程化建议

一.Faster-lio 是一种轻量级的激光雷达惯性里程计，用于激光雷达姿态跟踪和点云测绘。它

是在 FastLIO2 基础上开发的，速度提高了 1.5-2 倍左右。对于固态激光雷达，它可以达到近

1k-2khz 的运行帧率，对于典型的 32 线旋转激光雷达，它可以超过 100hz 运行帧率。

项目源码：https://github.com/gaoxiang12/faster-lio

论文：https://github.com/gaoxiang12/faster-lio/blob/main/doc/faster-lio.pdf

二.Fast-lio-sam 是进一步加速的 fastlio2，它的主要贡献：

1.对比 FAST_LIO_SLAM 与 FAST_LIO_LC 使用外部接入的 PGO 回环检测模块进行后端优

化 ，FAST_LIO_SAM 将 LIO-SAM 的后端 GTSAM 优化部分移植到 FAST-LIO2 的代码中，数

据传输处理环节更加清晰。

2.增加关键帧的保存，可通过 rosservice 的指令对地图和轨迹进行保存。

3.FAST_LIO_SLAM 中的后端优化，只使用了 GPS 的高层进行约束，GPS 的高层一般噪声比

较大，所以添加 GPS 的 XYZ 三维的 postion 进行 GPS 先验因子约束。

项目源码：https://github.com/kahowang/FAST_LIO_SAM

三、Fast_lio_localization 是一个基于 fastlio 的点云地图可以进行重定位的架构，它的特点包

括：

1.在预建点云地图中实现了实时三维全球定位。通过融合低频全局定位(约 0.5~0.2Hz)和

FAST-LIO 的高频里程计，整个系统的计算效率很高。

2.消除了里程计的累积误差。

3.初始定位可以由 RVIZ 的粗略手动估计提供，也可以由另一个传感器/算法提供姿态。

项目源码：https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION

四、FAST_LIO_LC 是一套带有回环检测的雷达里程计算法

项目源码： https://github.com/yanliang-wang/FAST_LIO_LC