Simple-LIO-SAM——（十）后端优化模块详解

发表于 更新于 分类于 ， 阅读次数： Changyan：

后端优化

Simple-LIO-SAM总体流程图

这是LIOSAM四大部分（点云去畸变、特征提取、预积分、后端优化）中最复杂的一个模块了，完全弄懂这个模块也就离完全理解LIOSAM不远了。 这个模块负责SLAM框架中的后端优化，由于LIOSAM中使用图优化作为后端优化算法，所以有时候笔者也会称这个模块为图优化模块。但是这个模块的 作用不单单是负责后端优化，还有1）、全局地图可视化；2）、回环检测等功能。

这个模块一共有三个主要线程执行具体功能，三个线程分别如下：

  1. 雷达里程计及后端优化线程

这个线程主要负责执行雷达到地图的匹配，得到较为准确的雷达里程计，然后将雷达里程计、回环检测因子加入因子图进行优化，得到全局优化的关键帧位姿。

  1. 回环检测线程

回环检测使用较为费时的ICP算法，因此作为一个单独的线程运行。当检测到合适的回环因子后，会加入全局缓存队列，在雷达里程计线程中被加入因子图优化。

  1. 全局地图可视化线程

LIOSAM中对于地图的存储格式是通过一系列关键帧的点云和位姿存储，而不是通过一整个点云存储。为了能够以较低的频率发布全局地图，后端优化模块单独有一个线程 对历史关键帧的点云根据最新的位姿转换到地图坐标系，然后将融合后的全局点云地图发布出去。

废话不多说，下面分别介绍这三个线程的主要流程和方法

雷达里程计及后端优化线程

这个线程的流程主要如下图所示

雷达里程计及后端优化线程流程图

这个线程是点云的回调函数，点云来自FeatureExtraction模块输出的经过特征提取后的点云。整个函数执行过程代码如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
// 特征点云回调函数，也是整个模块核心的计算函数
void laserCloudInfoHandler(const spl_lio_sam::msg::CloudInfo::SharedPtr msgIn)
{
    // 提取当前点云时间戳
    timeLaserInfoStamp = msgIn->header.stamp;
    timeLaserInfoCur = stamp2Sec(msgIn->header.stamp);

    // 提取当前点云的特征角点和特征平面点
    cloudInfo = *msgIn;
    pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerCur);
    pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfCur);

    /**
        * 一共三个线程使用到这把锁
        * 1. 雷达里程计线程，也就是当前线程
        * 2. 发布全局地图线程，执行关键帧点云拷贝转换操作
        * 3. 回环检测线程，执行关键帧姿态拷贝操作
    */
    std::lock_guard<std::mutex> lock(mtx);

    // 记录上一帧的时间戳，两帧之间时间间隔大于mappingProcessInterval才会进行处理
    static double timeLastProcessing = -1;
    if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
    {
        timeLastProcessing = timeLaserInfoCur;

        // 当前帧位姿初始化
        RCLCPP_DEBUG(get_logger(), "updateInitialGuess");
        updateInitialGuess();

        // 构建局部地图
        RCLCPP_DEBUG(get_logger(), "extractSurroundingKeyFrames");
        extractSurroundingKeyFrames();

        // 对当前帧点云做降采样
        RCLCPP_DEBUG(get_logger(), "downsampleCurrentScan");
        downsampleCurrentScan();

        // 将当前帧点云匹配到构建的局部地图，优化当前位姿
        RCLCPP_DEBUG(get_logger(), "scan2MapOptimization");
        scan2MapOptimization();

        // 计算是否将当前帧采纳为关键帧，加入因子图优化
        RCLCPP_DEBUG(get_logger(), "saveKeyFramesAndFactor");
        saveKeyFramesAndFactor();

        // 当新的回环因子或者GPS因子加入因子图时，对历史帧执行位姿更新
        RCLCPP_DEBUG(get_logger(), "correctPoses");
        correctPoses();

        // 发布激光历程计
        RCLCPP_DEBUG(get_logger(), "publishOdometry");
        publishOdometry();

        // 发布当前帧对齐到地图坐标系的点云和完整轨迹
        RCLCPP_DEBUG(get_logger(), "publishFrames");
        publishFrames();
    }
}

雷达里程计的最高频率

雷达里程计并不是对每一帧点云都进行处理，这里有一个时间阈值的判断

1
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)

mappingProcessInterval的默认参数是0.15s，所以在默认参数下，雷达里程计不会超过1/0.15Hz

点云匹配的初始值

基本所有的精细点云匹配都需要一个较好的初值。LIOSAM中的点云匹配使用高斯牛顿算法优化当前帧点云到局部地图的距离（具体算法见本项目博客的相关文章），基于最小二乘法的点云匹配更需要一个优良的初值。 回顾一下我们在ImageProjection也就是点云去畸变模块的内容，在点云去畸变模块，除了对点云做运动畸变校正之外，还从IMU原始输出的角度和预积分模块的输出找到与当前帧点云时间最相近的数据，加入发布的消息中跟随点云一起发布。

在后端优化模块中，会优先判断接收的消息中是否有设置好的预积分数据，若有，则使用该数据作为该帧点云的初始值，否则，使用IMU的角度增量作为初始值。当然，特殊情况是第一帧的时候会使用IMU输出的角度作为初始值。

点云匹配初始值选取

这里比较特殊的是，初始值是通过计算当前帧的位姿和上一帧的缓存位姿计算位姿增量，然后将该增量应用到上一帧优化后的里程计结果。

\[ T_{incre} = \hat{T}_{pre}^{-1}\hat{T}_{cur}\\ \bar{T}_{cur} = T^{opt}_{pre}T_{incre} \]

以使用预积分数据作为初始值为例，上面公式中的\(\hat{T}_{pre},\hat{T}_{cur}\)分别是上一次缓存和当前的消息中包含的预积分位姿；\(T_{incre}\)计算除了从上一帧到当前帧的位姿增量；\(T^{opt}_{pre}\)是上一帧经过点云匹配、图优化之后的里程计结果；\(\bar{T}_{cur}\)则是最终采用的当前帧点云的估计值。

构建局部点云地图

构建局部点云地图

局部点云地图简单说就是根据最后一帧关键帧，找到其空间、时间附近的一些其他关键帧，将这些关键帧的点云都使用对应的位姿转换到地图坐标系下，融合成地图坐标系下的局部地图，后面这个局部地图就是点云匹配的目标点云。这个函数主要是一些几何计算。

点云匹配

这部分用的算法比较复杂，具体参考本项目的另一篇文章Simple-LIO-SAM——点云匹配算法详解。 只要明白，经过这个步骤后，在第一步设置的点云位姿初始值已经被点云匹配算法更新为更准确的位姿。

点云匹配

因子图构建及优化

这个步骤稍微比较复杂，我们先理一下目前手头有什么：

  1. 经过点云匹配校正后的当前帧的位姿
  2. 回环检测得到的某一帧关键帧到另一帧关键帧的相对位姿（这个部分在回环检测线程进行，这里只需要当作现有数据即可）
  3. gps里程计（由于这部分需要额外的计算，暂且忽略gps的输入）

PS:这部分需要GTSAM的知识，可以参考本项目的另外一篇文章：Simple-LIO-SAM——GTSAM快速入门

LIOSAM使用上面两个位姿关系构建因子图，并使用因子图优化算法进行整体的优化，得到全局优化下的关键帧位姿

因子图构建流程图

关键帧的选择

并不是所有的雷达帧都会被当作关键帧，LIOSAM使用一种比较简单的方案判断是否将当前帧采纳为关键帧：只有与上一帧关键帧的位姿相差足够大才会采纳为关键帧。

添加激光里程计因子

激光里程计因子

回环因子

添加回环因子

设置因子图初值并优化

因子图优化

更新历史关键帧位姿

当有新的回环被检测到，该回环关系被加入因子图进行优化。由于回环关系是一种很强约束性的观测，因此，在观测到新的回环之后，因子图优化对历史关键帧的位姿变化会比较大，因此需要对历史关键帧的位姿一起做一次更新。

更新历史位姿

发布里程计及可视化点云

在原始的LIOSAM代码中这部分还有较多处理，但实际上并没有太多作用。在本项目代码spl中，已经去除了冗余部分。这部分就只是将计算出来的激光里程计往外发布。

回环检测线程

回环检测是SLAM中很重要的一个环节。通过检查当前位置是否已经是到达过的位置，并对重复到达的位置匹配计算位姿。如下图中，\(x_5\)与\(x_2\)够成回环，\(f_5(x_5,x_2)\)是两个位置间的相对位置关系。通过回环之间的约束，对因子图优化器提供很强的信息，使得整体的位姿估计达到全局最优的状态。

回环因子

LIOSAM中回环检测使用ICP算法，计算最新的关键帧与可能形成回环的位置的点云匹配，并只有在匹配分数非常好的情况下才采纳这个回环结果。由于这部分是比较耗费计算时间的，并且，回环关系只需要以比较低的状态加入因子图即可，因此，在LIOSAM的mapOptimization.cpp中是使用一个独立线程执行回环检测：

1
2
3
4
auto MO = std::make_shared<mapOptimization>(options);

// 回环检测独立线程
std::thread loopthread(&mapOptimization::loopClosureThread, MO);

同时，这个线程内部也默认被设为较低的执行频率（默认1Hz）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/**
  * 回环检测独立线程
  * 1. 由于回环检测中用到了点云匹配，较为耗时，所以独立为单独的线程运行
  * 2. 新的回环关系被检测出来时被主线程加入因子图中优化
*/
void loopClosureThread()
{
    if (loopClosureEnableFlag == false)
        return;

    // 默认 1
    rclcpp::Rate rate(loopClosureFrequency);
    while (rclcpp::ok())
    {
        rate.sleep();
        performLoopClosure();
        visualizeLoopClosure();
    }
}

回环检测线程的整体流程如下：

回环检测线程流程图

寻找回环对应的帧

当开始寻找新的回环关系时，会根据最后一帧关键帧找到是否有合适的对应帧作为回环帧。主要通过两个约束：

  1. 空间位置最近（使用kdtree做距离检索）
  2. 时间距离够远（去除时间过近的帧）

这个部分主要是函数detectLoopClosureDistance

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
/**
  * @brief 根据位置关系寻找当前帧与对应帧的索引
  * 1. 将最后一帧关键帧作为当前帧，如果当前帧已经在回环对应关系中，则返回（已经处理过这一帧了）。如果找到的回环对应帧相差时间过短也返回false。回环关系用一个全局map缓存
  * 2. 对关键帧3D位姿构建kd树，并用当前帧位置从kd树寻找距离最近的几帧，挑选时间间隔最远的那一帧作为匹配帧
  * 
  * @param latestID 传出参数，找到的当前帧索引，实际就是用最后一帧关键帧
  * @param closestID 传出参数，找到的当前帧对应的匹配帧
*/
bool detectLoopClosureDistance(int *latestID, int *closestID)
{
    int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
    int loopKeyPre = -1;

    // 确认最后一帧关键帧没有被加入过回环关系中
    auto it = loopIndexContainer.find(loopKeyCur);
    if (it != loopIndexContainer.end())
        return false;

    // 将关键帧的3D位置构建kdtree，并检索空间位置相近的关键帧
    std::vector<int> pointSearchIndLoop;
    std::vector<float> pointSearchSqDisLoop;
    kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
    // 寻找空间距离相近的关键帧
    kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
    
    // 确保空间距离相近的帧是较久前采集的，排除是前面几个关键帧
    for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
    {
        int id = pointSearchIndLoop[i];
        if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
        {
            loopKeyPre = id;
            break;
        }
    }

    // 如果没有找到位置关系、时间关系都符合要求的关键帧，则返回false
    if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
        return false;

    *latestID = loopKeyCur;
    *closestID = loopKeyPre;

    return true;
}

构建局部地图

找到匹配的关键帧id后，会在该关键帧的周围抽取其他关键帧构建局部点云地图。这个步骤跟雷达里程计线程中的局部地图构建是类似的，这里就不再赘述。

点云匹配

这里使用ICP直接对curKeyframeCloud(最后一帧关键帧)和preKeyframeCloud(回环对应帧周围构建的点云地图)做点云匹配，并得出当前帧到回环帧的转换矩阵\(T_{cur}^{pre}\)

添加回环关系到队列

由于回环关系检测跟因子图优化是两个线程，为了避免复杂的线程同步关系，在回环线程中用几个队列将回环关系缓存，在雷达里程计线程中从这几个队列提取相应的回环关系加入因子图。这里主要缓存几个内容：1）、回环索引；2）、回环关系的位姿转换矩阵（也就是ICP的结果）；3）、噪声因子（根据匹配的分数计算的噪声）

1
2
3
4
5
6
// 9. 将回环索引、回环间相对位姿、回环噪声模型加入全局变量
mtx.lock();
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();

全局地图可视化线程

这个线程主要是将所有历史关键帧，使用关键帧的位姿，全部转换到地图坐标系下，构建一个全局地图，并发布。这个线程完全是为了可视化，同时，处理所有的关键帧也需要很大计算量，因此，这个线程也被默认限制在5Hz的频率下执行。主要操作就是点云坐标系转换，这里就不赘述了。函数是publishGlobalMap 

1
2
3
4
5
6
7
8
9
10
/**
  * 发布全局地图点云，在全局地图可视化线程中调用
  * 1. 对所有关键帧3D位姿构建KD树
  * 2. 以最后一帧关键帧为索引找出一定半径范围内的所有关键帧
  * 3. 对找出的关键帧数量做降采样
  * 4. 对所有关键帧的点云做拼接（投影到地图坐标系）
  * 5. 对地图点云做降采样
  * 6. 发布全局地图点云
*/
void publishGlobalMap()

本项目所有章节

  1. Simple-LIO-SAM——（一）项目简介
  2. Simple-LIO-SAM——（二）环境搭建与运行
  3. Simple-LIO-SAM——（三）总体流程认识
  4. Simple-LIO-SAM——（四）utility文件解读
  5. Simple-LIO-SAM——（五）点云去畸变模块
  6. Simple-LIO-SAM——（六）特征提取模块
  7. Simple-LIO-SAM——（七）GTSAM快速入门
  8. Simple-LIO-SAM——（八）IMU预积分模块
  9. Simple-LIO-SAM——（九）点云匹配算法详解
  10. Simple-LIO-SAM——（十）后端优化模块详解