ROS-3DSLAM（十六）lvi-sam项目总结

2021SC@SDUSC

学习内容概览

本次的项目lvi-sam主要分为两个大的模块：lidar模块和visual模块。我们小组学习先进行了lidar模块的学习，然后进行的visual模块。每个模块都分成了若干小的部分，分给每个组员主要负责，学习完成后进行组内交流。

我在lidar模块负责的部分是imuPreintegration（imu预积分）和mapOptmization（图优化）；visual模块主要负责的是visual_loop（回环检测）。

imuPreintegration

imyPreintegration中主要的作用是将imu回传的数据进行预积分处理。

我对该节点的分析主要是通过它订阅的两个话题的回调函数展开的：

//nh ： ros::NodeHandle nh;   在utility.h中定义的
// 订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的 imu预计分量 ，预测每一时刻（imu频率）的imu里程计
//<sensor_msgs::Imu>：通过源码分析，<>内的类型为函数的第三个参数（函数参数）的参数，需要显式的声明出来
subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）
subOdometry = nh.subscribe<nav_msgs::Odometry>(PROJECT_NAME + "/lidar/mapping/odometry", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

imuHandler：订阅imu原始数据，用因子图优化的结果，施加两帧之间的 imu预计分量 ，预测每一时刻（imu频率）的imu里程计。

订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）。

可以理解为两帧雷达帧之间有若干的imu帧，通过将这些imu进行预积分处理，进行之后的帧优化工作。

发布了两个话题：都是关于imu里程计的信息。分别是里程计的信息和imu路径信息。

需要注意的是，接收到的原始imu数据和lidar激光帧都是在其原坐标系下的，在该节点实际参与计算时，需要将他们进行坐标系统一。

mapOptmization

mapOptmization的代码量比较大，但是当梳理清楚后可以发现该节点的所有工作都是围绕着对激光雷达帧的优化展开的。mapOptmization订阅了来自FeatureExtraction的当前激光帧点云信息，通过一系列步骤得到优化后的激光帧：

1. 对当前位姿进行初始化
   1. 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）。
   2. 如果是后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧的激光位姿。
2. 提取局部角点、平面点云集合，加入局部地图
   1. 对最近一帧的关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
   2. 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部地图中
3. 对第2步提取的当前激光帧角点、平面点集合降采样
4. scan-to-map优化当前帧位姿
5. 判断当前帧是否为关键帧；设置当前帧为关键帧并执行因子图优化
   1. 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
   2. 迭代30次（上限）优化
      1. 当前激光帧角点寻找局部地图匹配点
         1. 更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部地图中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
         2. 计算当前帧角点到平面的距离、垂线的单位向量，存储为角点参数
      2. 当前激光帧平面点寻找局部地图匹配点
         1. 更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部地图中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了。
         2. 计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
      3. 提取当前帧中与局部地图匹配上了的角点、平面点，加入同一集合
      4. 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前帧位姿，存transformTobeMapped
   3. 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll,pitch,约束z坐标
6. 更新因子图中历史关键帧的位姿，更新里程计轨迹（可以理解为用当前帧来优化历史帧）
   1. 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
   2. 添加激光里程计因子、GPS因子、闭环因子
   3. 执行因子图优化
   4. 得到当前帧优化后的位姿，位姿协方差
   5. 添加cloudKeyPoses3D,cloudKeyPoses6D,更新transformTobeMapped,添加当前关键帧的角点、平面点集合
7. 发布激光里程计
8. 发布里程计、点云、轨迹

其中imuPreintegration订阅的激光里程计话题就是该节点发布的。

• 在这里mapOptmization节点发布的优化后的激光雷达帧被imuPreintegration节点订阅，然后通过自身计算的imu预积分进一步优化激光雷达帧位姿，从而将imu里程计和激光里程计相融合。

• 注意：mapOptmization节点发布的信息同时被多个节点所订阅，我会在后面的节点数据流向中作详细的分析。

visual_odometry/visual_loop

该节点位于视觉部分。视觉部分主要分为三个节点，loop节点的作用是进行回环检测。

该节点主要订阅来自视觉部分的另一个节点estimator发布的话题，包括关键帧的位姿、位置和图像信息，根据这些信息，通过回环检测算法（词袋模型），检测闭环，从而优化全局位姿。

该节点在初始化的时候会将项目中的数据集加载到程序中（config下的brief开头的文件）。这些数据集是词袋模型中的字典集，使用的描述子是BRIEF描述子。

之后会开启一个并行的线程，用来接收新的关键帧，并对他进行处理，判断是否形成闭环。

该节点发布的信息主要是回环检测优化过后的关键帧的相关信息。

节点间交互

首先看一下整个项目的rqt_graph图：

我分析的部分的节点数据流向总的来说并不复杂。

imuPreintegtration

可以看到imuPreintegtration有2条入边，3条出边。

首先是imuPreintegtration节点，接收来着imu的原始数据和mapOptmization的数据，如何处理在上面的总结中都有提到，这里能体现出项目的雷达-惯导融合；而对于订阅该节点发布的话题的节点，通过在后续visual部分学习后，和同学进行交流后知道，被视觉部分的feature和odometry部分订阅，用来作雷达-视觉融合后的imu优化位姿；另一个则是被rvzi订阅，用来图像显示。

mapOptmization

mapOptmization订阅的话题包括点云信息和关键帧信息，这也与要优化的内容相匹配；而订阅该节点发布话题的节点较多，总结下来都是那些需要关键帧来做处理的节点。

visual_odometry/visual_loop

在这里插入图片描述

以visual_loop节点为中心，可以看到之前的mapOptmization订阅的关键帧信息，正是通过视觉部分回环检测后的关键帧信息，这里较好的体现出了视觉-雷达-惯导融合的部分：lidar部分的mapOptmization节点，用imu预积分的结果来优化从visual部分传来的关键帧数据。

该节点订阅的消息则很好判断：视觉里程计提供关键帧信息；imageProjection提供图片信息。

总结

在本次课程的总体学习中，我感觉除了对整个liv-sam项目的学习外，我还有很多其他收获：

• 阅读分析长代码的能力有了很大的提升。
• 对c++的一些深层次的语法的应用有了认识，如共享指针、重定义等等。
• 对linux系统和ROS系统的使用也得到了锻炼，尤其是在我们安装测试lvi-sam项目的时候，确实花费了一些功夫，但也从中总结了很多经验教训。
• 除了lvi-sam外，我还在本学期自学了其他SLAM的相关知识，以《SLAM14讲》书籍为线索，配合网上的各种博客教程，让我对整个SLAM有了更加全面、系统的学习，为日后在无人驾驶导航方面的继续深入学习打下了基础。
• 这次可能也让我认识到了团队的力量，队员之间合理分工、互相学习，就会发挥出1+1>2的能力