首先从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是传感器输入。
函数类名称接收消息发布消息imageProjectionimu_raw odometry/imu_incremental points_rawlio_sam/deskew/cloud_infoTransformFusionlio_sam/mapping/odometryodometry/imu_incrementalIMUPreintegrationlio_sam/mapping/odometry_incrementalimu_rawodometry/imu_incrementalFeatureExtractionlio_sam/deskew/cloud_infolio_sam/feature/cloud_infomapOptimizationlio_sam/feature/cloud_infolio_sam/mapping/odometrylio_sam/mapping/odometry_incremental
程序tf树结构如下,和一般的机器人导航系统一样使用了map,odom,base_link 和lidar_link四个坐标系,但是目前的代码是为了跑数据集设置的参数,map和odom,base_link 和lidar_link两对坐标系都是重合的。
主函数部分只是创建了imageProjection类对象IP,就开始等待回调函数了。类初始化部分设置了ROS中需要接收和发送的消息,分配控件和设置参数。函数的重点就是里程计,IMU和雷达三个回调函数。
这两个回调函数都很简单,用std::lock_guard互斥锁进行保护,将接收的消息加入队列中。
按顺序介绍cloudHandler中各个函数的作用。
cachePointCloud 缓存点云,同时在第一次运行是检查激光是否有ring和time两个参数。deskewInfo 处理IMU和里程计信息,对IMU的速度进行累加。projectPointCloud 将点投影到range image上,利用IMU的旋转,对点进行去畸变,同时变换到第一帧的坐标系中。cloudExtraction和publishClouds 按照Range Image的顺序,提取和发布点云。imuPreintegration.cpp中包含两个类,IMUPreintegration和TransformFusion。
IMUPreintegration类的作用是计算预积分,但是为了计算精度,对IMU数据会做三次处理,预积分,预积分并使用GTSAM优化IMU估计和偏差,最后利用更新的偏差再次进行预积分。在构造函数中,使用imu参数初始化了 GTSAM优化框架。主要的程序处理依然在两个回调函数中。
&emsm;接收imu数据进行预积分,发送预积分结果,imu增量信息odometry/imu_incremental。值得注意的是,在发送的nav_msgs::Odometry类型的数据中,速度在imu坐标系下,而位姿在lidar坐标系下,不知道为什么要这么处理。调查发现代码其他地方并没有使用速度消息。
优化每进行一百次,去除过去的观测,只保留现在优化的偏差继续用GTSAM进行优化。 预积分imu,利用GTSAM优化偏差和状态。判断当前偏差和状态,如果误差太大,就重置重新优化。最后利用之前优化的偏差重新进行预积分。
这个类的主要作用是向rviz发送两个位姿变换,当前位姿和imu path。 首先,发送odom到map的固定变换,代码中是重合的。接收mapOptimization发出的里程计信息,结合IMUPreintegration发出的IMU预积分增量,发送odom到lidar的位姿变换和odom的里程计信息。同时根据里程计信息,发布path消息。
类似于LOAM的特征提取测量,在判断遮挡之后,对每一个SCAN分六段根据smooth选取线特征和面特征,对面特征进行降采样。
代码运行分为三个线程,mapOptimization的回调函数,回环检测,以及地图输出。先从后两个比较简单的说起。在代码里还可以看到,作者还计划引入通过用户交互来设置回环。
通过将关键帧构建KD tree来搜寻临近的关键帧,选择与最新的关键帧位姿临近而且时间差超过阈值的关键帧作为回环候选。 使用候选帧附近的帧构建局部地图,对局部地图和当前帧进行降采样。使用icp估计当前帧到局部地图的位姿变换,并设置因子图相关参数。
发布全局地图和在程序运行结束之后保存点云和轨迹。
每隔一段时间,对地图进行优化,分别介绍调用的几个函数的功能。
updateInitialGuess 根据imu预积分的结果设置优化初值。extractSurroundingKeyFrames和downsampleCurrentScan 通过构建kd tree找到临近帧和时间最近的几帧构建局部地图,对局部地图和当前帧进行降采样。scan2MapOptimization 迭代优化当前帧到局部地图的位姿变化。每次迭代通过构建kd tree找到距离当前帧线和面特征最近的五个线和面特征点,根据这五个点做直线和平面拟合,计算点到直线和平面的距离,同时计算距离的单位方向向量。通过高斯牛顿法优化位姿变换。saveKeyFramesAndFactor 如果角度或者距离变化较大,将当前帧选为关键帧,同时根据里程计和回环节点在ISAM中进行全局优化。保存关键帧位姿。correctPoses 修正回环导致的位姿变化。