3d激光SLAMLIO-SAM框架介紹

LIO-SAM的全稱是：Tightly-coupled Lidar Inertial Odometry via Smoothing and
Mapping

從全稱上可以看出，該算法是一個(gè)緊耦合的雷達(dá)慣導(dǎo)里程計(jì)（Tightly-coupled Lidar Inertial
Odometry），借助的手段就是利用GT-SAM庫(kù)中的方法。

LIO-SAM 提出了一個(gè)利用GT-SAM的緊耦合激光雷達(dá)慣導(dǎo)里程計(jì)的框架。

實(shí)現(xiàn)了高精度、實(shí)時(shí)的移動(dòng)機(jī)器人的軌跡估計(jì)和建圖。

本篇博客重點(diǎn)解讀LIO-SAM框架下IMU預(yù)積分功能數(shù)據(jù)初始化代碼部分

LIO-SAM 的代碼主要在其主目錄內(nèi)的src文件夾下的四個(gè)cpp文件，分別是：

featureExtraction.cpp

imageProjection.cpp

imuPreintegration.cpp

mapOptmization.cpp

每個(gè)cpp文件是一個(gè)獨(dú)立的ROS節(jié)點(diǎn)，對(duì)應(yīng)著下圖的四個(gè)模塊

lio-sam的所有文件即對(duì)應(yīng)功能在下面做了如下總結(jié)：

lio-sam：│ CMakeLists.txt #項(xiàng)目工程配置文件，可以知道作者用了哪些第三方庫(kù)及cpp生成了什么執(zhí)行文件

│ LICENSE #軟件版權(quán)

│ package.xml #ROS包配置文件

│ README.md #項(xiàng)目工程說明文件：文件構(gòu)成、依賴、運(yùn)行等

│ ├─config│ │ params.yaml #參數(shù)文件

│ │ │ └─doc #存儲(chǔ)效果圖、流程圖、論文等

│ │ │ └─kitti2bag #將kitti數(shù)據(jù)集轉(zhuǎn)換成bag格式

│ kitti2bag.py│ README.md│ ├─include│ utility.h #參數(shù)服務(wù)器類，初始化參數(shù)；各類公用函數(shù)

│ ├─launch│ │ run.launch #總運(yùn)行l(wèi)aunch文件

│ │ │ └─include #分模塊運(yùn)行文件

│ │ module_loam.launch│ │ module_navsat.launch│ │
module_robot_state_publisher.launch│ │ module_rviz.launch│ │ │ ├─config
#存儲(chǔ)rviz參數(shù)文件和機(jī)器人坐標(biāo)系參數(shù)

│ │ rviz.rviz│ │ robot.urdf.xacro│ ├─msg│ cloud_info.msg #自定義ROS數(shù)據(jù)格式

│ ├─src #源文件

│ featureExtraction.cpp #提取雷達(dá)線面特征，發(fā)布雷達(dá)點(diǎn)云

│ imageProjection.cpp#訂閱提取的雷達(dá)點(diǎn)云、IMU數(shù)據(jù)和IMU里程計(jì)數(shù)據(jù)，對(duì)雷達(dá)做畸變矯正，進(jìn)行雷達(dá)前端里程計(jì)位姿粗估計(jì)的發(fā)布（以IMU頻率）

│ imuPreintegration.cpp#IMU預(yù)積分，訂閱雷達(dá)里程計(jì)和IMU數(shù)據(jù)，估計(jì)IMU偏置，進(jìn)行雷達(dá)里程計(jì)、IMU預(yù)積分因子的圖優(yōu)化，輸出IMU里程計(jì)。

│ mapOptmization.cpp #訂閱雷達(dá)前端信息、GPS信息，進(jìn)行點(diǎn)云配準(zhǔn)，進(jìn)行雷達(dá)里程計(jì)、全局GPS、回環(huán)檢測(cè)因子的圖優(yōu)化。

│ └─srv save_map.srv

本篇主要解讀 IMU預(yù)積分部分代碼，也就是

imuPreintegration.cpp#IMU預(yù)積分，訂閱雷達(dá)里程計(jì)和IMU數(shù)據(jù)，估計(jì)IMU偏置，進(jìn)行雷達(dá)里程計(jì)、IMU預(yù)積分因子的圖優(yōu)化，輸出IMU里程計(jì)。