LIO-SAM代码阅读
文章目录
- 程序整体框架
-
- roslaunch文件结构
- 设置文件
- 多线程间消息交互
- tf树结构
- imageProjection.cpp
-
- imuHandler和odometryHandler
- cloudHandler
- imuPreintegration.cpp
-
- IMUPreintegration类
-
- imuHandler函数
- odometryHandler函数
- TransformFusion类
- featureExtraction.cpp
- mapOptmization.cpp
-
- loopClosureThread
- visualizeGlobalMapThread
- laserCloudInfoHandler
- 代码细节
项目网址：https://github.com/TixiaoShan/LIO-SAM；
参考网址：
- https://www.zhihu.com/people/gu-du-huan-zhe-90/posts LIO-SAM源码阅读与分析
- https://zhuanlan.zhihu.com/p/176976425 Lio-sam 流程浅析
- https://github.com/cuitaixiang/LOAM_NOTED/tree/master/src/loam_velodyne LeGO-LOAM代码注释版
- https://wykxwyc.github.io/2019/08/01/The-Math-Formula-in-LeGO-LOAM/ LeGO-LOAM公式推导
程序整体框架
roslaunch文件结构
首先从roslaunch文件来了解程序运行的整体框架。LIO-SAM运行涉及到的roslaunch文件有四个，一个顶层roslaunch文件和其调用的四个roslaunch文件。
- run.launch
调用了其他四个launch文件，设置了参数文件地址。 - module_loam.launch
类似于LeGO-LOAM启动了四个节点。 - module_robot_state_publisher.launch
调用ROS的robot_state_publisher节点，输出机器人参数到tf。 - module_navsat.launch
调用ROS中robot_localization包中的ekf_localization_node和navsat_transform_node节点，对机器人进行融合GPS信号的定位。 - module_rviz.launch
按照预设的参数运行Rviz。
设置文件
设置文件包括params.yaml和utility.h两个文件
- params.yaml
额外记一个参数，extrinsicRot和extrinsicRPY都是用来把IMU测量转换到lidar坐标系下。之所以要两个是因为在作者使用的IMU中，加速度坐标系和角度坐标系不一样，所以要分开使用。 - utility.h
定义了基类供其他函数继承，设置了变量和从yaml文件读取的参数，还有一些工具函数和雷达相关设定。
多线程间消息交互
去除了gps消息，未使用的消息和用于rviz显示的消息。其中，imu_raw和points_raw是传感器输入。
| 函数类名称 | 接收消息 | 发布消息 |
|---|---|---|
| imageProjection | imu_raw odometry/imu_incremental points_raw |
lio_sam/deskew/cloud_info |
| TransformFusion | lio_sam/mapping/odometry odometry/imu_incremental |
|
| IMUPreintegration | lio_sam/mapping/odometry_incremental imu_raw |
odometry/imu_incremental |
| FeatureExtraction | lio_sam/deskew/cloud_info | lio_sam/feature/cloud_info |
| mapOptimization | lio_sam/feature/cloud_info | lio_sam/mapping/odometry lio_sam/mapping/odometry_incremental |
tf树结构
程序tf树结构如下，和一般的机器人导航系统一样使用了map，odom，base_link 和lidar_link四个坐标系，但是目前的代码是为了跑数据集设置的参数，map和odom，base_link 和lidar_link两对坐标系都是重合的。
imageProjection.cpp
主函数部分只是创建了imageProjection类对象IP，就开始等待回调函数了。类初始化部分设置了ROS中需要接收和发送的消息，分配控件和设置参数。函数的重点就是里程计，IMU和雷达三个回调函数。
imuHandler和odometryHandler
这两个回调函数都很简单，用std::lock_guard互斥锁进行保护，将接收的消息加入队列中。
cloudHandler
按顺序介绍cloudHandler中各个函数的作用。
- cachePointCloud
缓存点云，同时在第一次运行是检查激光是否有ring和time两个参数。 - deskewInfo
处理IMU和里程计信息，对IMU的速度进行累加。 - projectPointCloud
将点投影到range image上，利用IMU的旋转，对点进行去畸变，同时变换到第一帧的坐标系中。 - cloudExtraction和publishClouds
按照Range Image的顺序，提取和发布点云。
imuPreintegration.cpp
imuPreintegration.cpp中包含两个类，IMUPreintegration和TransformFusion。
IMUPreintegration类
IMUPreintegration类的作用是计算预积分，但是为了计算精度，对IMU数据会做三次处理，预积分，预积分并使用GTSAM优化IMU估计和偏差，最后利用更新的偏差再次进行预积分。在构造函数中，使用imu参数初始化了
GTSAM优化框架。主要的程序处理依然在两个回调函数中。
imuHandler函数
&emsm;接收imu数据进行预积分，发送预积分结果，imu增量信息odometry/imu_incremental。值得注意的是，在发送的nav_msgs::Odometry类型的数据中，速度在imu坐标系下，而位姿在lidar坐标系下，不知道为什么要这么处理。调查发现代码其他地方并没有使用速度消息。
odometryHandler函数
优化每进行一百次，去除过去的观测，只保留现在优化的偏差继续用GTSAM进行优化。
预积分imu，利用GTSAM优化偏差和状态。判断当前偏差和状态，如果误差太大，就重置重新优化。最后利用之前优化的偏差重新进行预积分。
TransformFusion类
这个类的主要作用是向rviz发送两个位姿变换，当前位姿和imu path。
首先，发送odom到map的固定变换，代码中是重合的。接收mapOptimization发出的里程计信息，结合IMUPreintegration发出的IMU预积分增量，发送odom到lidar的位姿变换和odom的里程计信息。同时根据里程计信息，发布path消息。
featureExtraction.cpp
类似于LOAM的特征提取测量，在判断遮挡之后，对每一个SCAN分六段根据smooth选取线特征和面特征，对面特征进行降采样。
mapOptmization.cpp
代码运行分为三个线程，mapOptimization的回调函数，回环检测，以及地图输出。先从后两个比较简单的说起。在代码里还可以看到，作者还计划引入通过用户交互来设置回环。
loopClosureThread
通过将关键帧构建KD tree来搜寻临近的关键帧，选择与最新的关键帧位姿临近而且时间差超过阈值的关键帧作为回环候选。
使用候选帧附近的帧构建局部地图，对局部地图和当前帧进行降采样。使用icp估计当前帧到局部地图的位姿变换，并设置因子图相关参数。
visualizeGlobalMapThread
发布全局地图和在程序运行结束之后保存点云和轨迹。
laserCloudInfoHandler
每隔一段时间，对地图进行优化，分别介绍调用的几个函数的功能。
- updateInitialGuess
根据imu预积分的结果设置优化初值。 - extractSurroundingKeyFrames和downsampleCurrentScan
通过构建kd tree找到临近帧和时间最近的几帧构建局部地图，对局部地图和当前帧进行降采样。 - scan2MapOptimization
迭代优化当前帧到局部地图的位姿变化。每次迭代通过构建kd tree找到距离当前帧线和面特征最近的五个线和面特征点，根据这五个点做直线和平面拟合，计算点到直线和平面的距离，同时计算距离的单位方向向量。通过高斯牛顿法优化位姿变换。 - saveKeyFramesAndFactor
如果角度或者距离变化较大，将当前帧选为关键帧，同时根据里程计和回环节点在ISAM中进行全局优化。保存关键帧位姿。 - correctPoses
修正回环导致的位姿变化。
代码细节
- respawn参数
在roslaunch文件运行节点时 ，设置节点在退出后重新启动。
respawn="true"(optional, default: False)
Restart the node automatically if it quits.
- ros::NodeHandle::param函数
从参数服务器获取参数，不能获取的情况下设置为默认值，返回是否成功获取参数。 - Eigen::Map函数
通过指针从C++数组构造Eigen数组，也可以像EIgen其他数据结构一样使用Map。 - tcpNoDelay参数
If a TCP transport is used, specifies whether or not to use TCP_NODELAY to provide a potentially lower-latency connection. - ros回调函数
参考：http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning
ROS::spin()和ROS::spinonce都是为了单线程设置的，如果要同时处理多个消息，应该使用MultiThreadedSpinner或者AsyncSpinner。spin和spinonce的区别在于spin有一个内置的等待时间。 - 使用imu修正位姿
在进行位姿估计和发布增量位姿的时候，都会和imu进行融合更新，但是权重很低。