返回

LIO-SAM 代码分析(四):mapOptimization

作为对 mapOptimization.cpp 的补充说明,主要是进行逻辑上的梳理以及一些细节的整理。

简介

LIO-SAM 的完整注释代码以及流程图见 LIO-SAM With Chinese Comments

mapOptimization.cpp 整体的代码逻辑可以参考下图结合代码注释进行阅读:

mapOptimization 流程图
mapOptimization 流程图

期间主要维护两个线程(还有一个可视化线程,这里不做详细介绍):

  • 主线程:接收点云消息,进行点云地图匹配估计,过程包括:构建局部地图,计算特征残差和雅可比,进行高斯牛顿优化。同时维护全局因子图优化,在位姿估计不好的时候利用 GPS 和回环消息进行全局位姿优化。
  • 回环检测线程:检测最新一个关键帧和地图之间是否存在回环,主要的方式是利用 ICP 匹配,因此需要一个较好的位姿估计初值

后端建图中节省计算时间的小技巧

LIO-SAM 作者在 mapOptimization.cpp 使用了不少方法来节省计算资源,这里可以参考借鉴一下:

  1. 在提取局部地图时,需要利用关键帧位姿将点云转换至世界坐标系下,如果每次都重复计算会浪费资源,因此作者维护一个世界坐标系点云的缓存,如果在缓存中找到之前转换好的点云可以直接使用。不过要注意,在进行回环检测之后需要将位姿发生变化的关键帧的点云清除。(作者是直接将缓存清空了)
  2. 要经常利用点云下采样来控制计算规模。作者不仅在常见的特征点云、局部地图等进行了下采样,还将位姿信息也作为一种点格式保存在点云里,因此在需要的时候也可以利用体素滤波对大量位姿进行下采样。具体可以参考函数 void extractNearby()
  3. 对很多点云中的遍历操作,如果循环中的每个流程是互相独立的话,可以用 OpenMP 进行并行处理。

其他注意事项

  • 整体框架在对 GPS 信息的处理上比较保守,只有当目前状态估计才构建 GPS 因子。而且由于 GPS 信息在高度方向上漂移较大,默认情况下只利用 GPS 消息中的 x, y 坐标
  • 在对当前位姿估计完成后,会按一定权重和 IMU 消息的权重,由于考虑 IMU 对 roll, pitch 较敏感,而对 yaw 不敏感,因此只融合 IMU 中的 roll 和 pitch 信息。
  • 在进行特征残差的雅可比计算时,由于利用 LOAM 的原始代码,需要一些旋转轴的特殊处理,具体过程可以参考这篇博客:基于 LOAM 的方法中平面点和边缘线的残差构建以及雅可比推导
Built with Hugo
Theme Stack designed by Jimmy