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

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

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

實現(xiàn)了高精度、實時的移動機器人的軌跡估計和建圖。

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

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

featureExtraction.cpp

imageProjection.cpp

imuPreintegration.cpp

mapOptmization.cpp

每個cpp文件是一個獨立的ROS節(jié)點，對應(yīng)著下圖的四個模塊

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

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

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

│ package.xml #ROS包配置文件

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

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

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

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

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

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

│ │ │ └─include #分模塊運行文件

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

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

│ ├─src #源文件

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

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

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

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

│ └─srv save_map.srv

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

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