上篇通过实验对比分析了是否启用后端优化与是否启用回环检测时的建图效果,并对后端优化与回环检测的作用进行了理论分析.

这篇文章将为大家解读一下后端优化以及回环检测是如何实现的.

代码的注释写完挺长时间了, 但是最近很忙一直没倒出功夫来写文章…

由于公众号不适合大段的代码, 因此本文只是通过文字来简述 Karto 的后端优化与回环检测的实现过程.

更详细的代码解析可以去看我的这篇文章:

karto探秘之open_karto 第四章 — 回环检测与后端优化:
https://blog.csdn.net/tiancailx/article/details/117109400

上文无图做好准备…

代码解读

主函数中与后端优化和回环检测相关的代码如下所示:

  kt_bool Mapper::Process(LocalizedRangeScan* pScan)
  {
      // 省略...
 
      if (m_pUseScanMatching->GetValue())
      {
        // add to graph 
        m_pGraph->AddVertex(pScan);
 
        //边是具有约束关系的两个顶点，在 AddEdges中的操作有： 寻找可以连接的两个帧，计算scanMatch，同时保存scanMatch的结果到Graph之中，然后添加到SPA里面
        m_pGraph->AddEdges(pScan, covariance);
 
        m_pMapperSensorManager->AddRunningScan(pScan);  // 这里面有RunningScan的维护，即删除不合适的头
 
        //进行回环检测的步骤
        if (m_pDoLoopClosing->GetValue())
        {
          std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
          const_forEach(std::vector<Name>, &deviceNames)
          {
            m_pGraph->TryCloseLoop(pScan, *iter);
          }
        }
      }
      m_pMapperSensorManager->SetLastScan(pScan);   //每一次的 LastScan都是一个正常的pScan，而不是空的
      return true;
    }
    return false;
  }

进行完扫描匹配之后,得到了当前scan的最优位姿. 之后向图结构中添加顶点和边, 再之后是进行回环检测.

1 向图结构中添加节点

之后,进行后端的第一步,将当前scan作为节点添加到图结构中.

这里的图结构有2个,
一个是 karto里的Graph类, 用于查找回环时,在这个图上做广度优先搜索.

这里的Graph::AddVertex() ，指的是调用MapperGraph的基类Graph 的AddVertex()函数，进行节点的保存．

第二个就是稀疏位姿图spa中的图结构了, 用于做全局位姿优化.

2 向图结构中添加边(约束)

向位姿图中添加边一共存在三种方式:

第一种: LinkScans()

将前一帧雷达数据与当前雷达数据连接，添加边约束. 调用一次SpaSolver::AddConstraint().

第二种 LinkChainToScan()

karto中将 一定范围内的 多个连续的 scan 称为一条chain, 中文不好翻译…就直接这么叫吧.

running scans 是在当前scan之前的连续的20m范围内的scan.

将running scans 作为一条chain, 将当前scan与这个chain添加一条边, 在所有的running scans中找到距离当前scan最近的一个scan, 再调用 LinkScans() 将这两个scan添加一条边.

第三种 LinkNearChains()

首先Graph图结构中搜索与当前scan的距离在一定阈值范围内的所有的scan, 这些scan被karto称为near scan .

然后根据near scan的index, 为每一个near scan 扩充成为一条chain, 以index增加 和 index减小 两个方向将 near scan 扩充成为一条chain. 同时, 这条chain 不可以包含当前的scan, 这种chain被成为 near chain.

FindNearChains() 函数调用FindNearLinkedScans() 来获取 near scan, 再对所有的near scan 扩充成为 near chain , 就会得到很多个 near chain , 之后通过将当前scan与这个chain进行扫描匹配, 计算下匹配得分, 如果匹配得分高于阈值就说明是个有效的 near chain.

LinkNearChains() 函数首先调用 FindNearChains() 获取 near chain .

之后再对每一个有效的near chain 与 当前scan 添加边, 每一个near chain 会调用一次 LinkChainToScan().

这其实也算是个回环检测, 只不过这个搜索的范围会小一些.

3 回环检测与位姿图优化

FindPossibleLoopClosure() 首先调用 FindNearLinkedScans() 获取当前节点的 near scan, 这些near scan 不可以作为回环.

这块我没搞懂, 可能是因为 near scan 已经在上边的程序中使用过了. 但是这块的搜索距离又和上边的不一样… 不理解.

之后, 对所有之前保存的scan做依次遍历, 计算与当前scan的距离.

如果距离小于m_pLoopSearchMaximumDistance, 那说明这附近可能是个存在回环的区域, 在这个区域范围内时向chain中添加数据.

当距离已经大于 m_pLoopSearchMaximumDistance , 说明已经出了回环可能存在的区域, 当出了这个区域时停止向chain添加scan,并判断这个chain是否是个有效的chain

有效的chain 要满足几个条件:

  • 要满足连续的几个候选解都在这个范围内才能是一个有效的chain.
  • 如果 pCandidateScan 在 nearLinkedScans 中, 就将这个chain删掉.
  • chain中的scan个数大于阈值m_pLoopMatchMinimumChainSize

如果找到了一个有效的chain, 程序就先退出, 并将当前处理到的scan的index返回.

TryCloseLoop() 函数首先调用 FindPossibleLoopClosure() 获取可能是回环的 chain , 然后使用当前scan与这个chain进行粗扫描匹配，如果响应值大于阈值，协方差小于阈值，再进行精匹配，如果精匹配的得分（响应值）大于阈值，则找到了回环

如果得分大于阈值, 就认为找到了一个回环, 调用LinkChainToScan() 函数添加一条边, 并紧接着调用 CorrectPoses() 函数执行全局优化,重新计算所有位姿.

之后再调用FindPossibleLoopClosure() 函数, 按照上次的index继续进行回环检测与匹配, 如果有回环了再进行扫描匹配. 直到index为最后一个scan的索引为止.

CorrectPoses() 是进行位姿图优化求解的地方。将这个函数放在了 回环检测的里边，也就是说，只有在回环检测通过（2帧scan匹配得分大于阈值时）的情况下，才进行一次图优化求解。

4 广度优先搜索算法

添加边时的LinkNearChains()函数 与 回环检测时的 FindPossibleLoopClosure()函数 都会调用 FindNearLinkedScans() 函数.

只不过前者是使用 FindNearLinkedScans() 的结果, 后者是将 FindNearLinkedScans() 的结果排除掉.

FindNearLinkedScans() 函数 是使用广度优先搜索算法, 在图结构中找到与当前节点距离在一定范围内的所有节点.

经常刷 leetcode 的同学可能会对这个算法很熟悉, 但是在slam中挺少遇到的, 所以这里将这一块拿出来着重讲一下.

所以, 同学们, 刷 leetcode 不是毫无作用的, 那里的算法是真正可以用到实际项目中的. slam 里就用到了.

Traverse()

下边这段代码就是 广度优先搜索算法 的具体实现.

实现的思路就是,

将当前scan放入 toVisit 这个队列中, 之后开始循环, 知道 toVisit 为空为止.

循环里会依次进行如下操作:

取出 toVisit 这个队列 的第一个数据pNext, 将其加入到 seenVertices 这个set 中, 表示已经使用过了的节点的集合.

计算pNext 与当前 scan 的距离, 如果距离小于阈值就加入到 validVertices ,这个vector中, 作为一个结果.

之后使用 GetAdjacentVertices()函数 获取 pNext 在图结构中的所有下一层相邻的节点, 将这些节点不在 seenVertices 中的加入到 toVisit 队列 以及 seenVertices集合 中.

依次循环, 从而完成了广度优先搜索. 其实具体实现思路是很简单的, 但是自己没刷过这种题, 就是不会做.

    /**
     * Traverse the graph starting with the given vertex; applies the visitor to visited nodes
     * 广度优先搜索算法,查找给定范围内的所有的节点
     */
    virtual std::vector<T *> Traverse(Vertex<T> *pStartVertex, Visitor<T> *pVisitor)
    {
        std::queue<Vertex<T> *> toVisit;
        std::set<Vertex<T> *> seenVertices;
        std::vector<Vertex<T> *> validVertices;

        toVisit.push(pStartVertex);
        seenVertices.insert(pStartVertex);

        do
        {
            Vertex<T> *pNext = toVisit.front();
            toVisit.pop();
            // 距离小于阈值就加入到validVertices中
            if (pVisitor->Visit(pNext))
            {
                // vertex is valid, explore neighbors
                validVertices.push_back(pNext);

                // 获取与这个节点相连的所有节点
                std::vector<Vertex<T> *> adjacentVertices = pNext->GetAdjacentVertices();
                forEach(typename std::vector<Vertex<T> *>, &adjacentVertices)
                {
                    Vertex<T> *pAdjacent = *iter;

                    // adjacent vertex has not yet been seen, add to queue for processing
                    if (seenVertices.find(pAdjacent) == seenVertices.end())
                    {
                        // 如果没有使用过,就加入到toVisit中,同时加入seenVertices以防止重复使用
                        toVisit.push(pAdjacent);
                        seenVertices.insert(pAdjacent);
                    }
                }
            }
        } while (toVisit.empty() == false);

        // 将结果保存成vector
        std::vector<T *> objects;
        forEach(typename std::vector<Vertex<T> *>, &validVertices)
        {
            objects.push_back((*iter)->GetObject());
        }

        return objects;
    }

5 总结

  1. karto的后端优化是和回环检测结合在一起的，首先向SPA中添加顶点，再通过3种方式向SPA中添加边结构，然后试着找回环。

  2. 找回环：首先通过坐标距离小于阈值, 并且形成的chain中scan的个数大于阈值，才算是一个候选的回环chain。然后使用当前scan与这个chain进行粗扫描匹配，如果响应值大于阈值，协方差小于阈值，再进行精匹配，如果精匹配的得分（响应值）大于阈值，则找到了回环。

  3. 找到了回环则将回环约束作为边结构添加到SPA中，然后进行图优化求解，更新所有scan的坐标。

  4. 通过 扫描匹配 与 后端优化环节可知，整个open_karto没有维护地图, 每次扫描匹配的地图都是通过一系列的scan新生成的, 是非常省资源的。
    最后的栅格地图是通过所有优化后的scan进行生成的，在slam_karto进行进行调用，不用维护整个栅格地图，非常的省cpu与内存。

6 Next

本篇文章通过文字简要说明了Karto的后端优化与回环检测的实现过程. 同时对其中使用到的 广度优先搜索算法 着重提了一下.

下一篇文章将通过 G2O或者Ceres 对Karto的后端优化器进行重写, 已达到学习的效果.

文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，可以在公众号中添加我的微信，进激光SLAM交流群，大家一起交流SLAM技术。

如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。

在这里插入图片描述