Karto SLAM之open_karto代码学习笔记（二）

前言

Karto SLAM之open_karto代码学习笔记（一）中介绍了karto slam中的各种类的作用，本篇博客主要介绍karto 算法中最重要的函数
kt_bool Mapper::Process(LocalizedRangeScan* pScan)

0. 代码地址

原始open_karto地址:
https://github.com/ros-perception/open_karto
https://github.com/ros-perception/slam_karto

加入了详细中文解释的open_karto地址:
https://github.com/kadn/open_karto

1.代码解析

process函数是karto算法的核心函数，该函数位于open_karto/src/Mapper.cpp中。
该函数的作用是处理当前激光数据帧，包含的主要过程有：

1. 初始化Lidar的相关参数（如范围等）
   Initialize(pLaserRangeFinder->GetRangeThreshold());
2. 获取上一帧激光数据帧
   LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName());
3. 通过和上一帧数据比较判断当前帧是否能作为关键帧，如果不能，该函数直接返回，不作处理

      if (!HasMovedEnough(pScan, pLastScan))
      {
        return false;
      }

4. 能作为关键帧有两种情况：1 是当前帧是第一帧 2是当前帧和上一帧相比在时间上或者距离上相距较远
5. 如果当前帧是第一帧，则不执行MatchScan操作，否则执行。MatchScan是用来将当前帧和前面的帧（多个）进行比较，根据odometry给出的初始位姿，在初始位姿附近找到更合适的位姿作为机器人移动位姿，同时返回该位姿下的response（衡量标准）以及covariance（可信度），具体内容在后面介绍

   m_pSequentialScanMatcher->MatchScan(pScan,
                                       m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
                                       bestPose,
                                       covariance);
   pScan->SetSensorPose(bestPose);   //出来了最好的位置`

6. 将当前帧加入数据库中
7. 将当前帧设置为图优化的顶点
   m_pGraph->AddVertex(pScan);
8. 将当前帧执行 添加边的操作，边和顶点是图优化的重要概念，可参考SLAM十四讲
   m_pGraph->AddEdges(pScan, covariance);
9. 将当前帧添加到runningScan中，并进行维护. runningScan即是第5步中被比较的对象，也即是“前面的帧”
   m_pMapperSensorManager->AddRunningScan(pScan);
10. 闭环检测
    m_pGraph->TryCloseLoop(pScan, *iter);
11. 将当前帧设置为 lastScan，函数返回

1.1 HasMovedEnough(pScan, pLastScan)

通过比较pScan和pLastScan之间的空间和时间距离，判断当前帧是否可以作为关键帧

1.2 MatchScan 见下一内容，本节介绍简单的函数

1.3 AddVertex(pScan) 添加顶点

1.4 AddEdges(pScan, covariance) 添加边

AddEdges是将相邻的scan与scan作为边传递给优化器。

1.4.1 如何判定相邻？

1. barycenter 指的是一个scan的激光点的世界坐标形成的boundingbox的中心
2. 当两个scan的barycenter的距离小于x m即认为相邻.

1.4.2 怎么找？FindNearChains

1. 使用FindNearChains会调用FindNearLinkedScans，会调用Traverse， Traverse使用广度优先遍历来找到相邻的帧
2. FindNearLinkedScans找的是scans， FindNearChains找的是 chains
3. scans 的结果可能是 5， 109， 78. 而chain的结果则会是 4-5-6-7-8, 或者 104-105-106-107-108-109-110-111

1.4.3 添加边的过程

1. LinkNearChains调用 FindNearChains找到要添加的帧，LinkChainToScan将其加入SPA优化器，因为这些帧被优化过了，所以就不需要再在 LoopClosure中找这些帧了

1.4.4 备注

1. AddEdges并不是都要调用 LinkNearChains来添加，也可能直接调用LinkScans来添加边。
2. LoopClosure的匹配帧不能在 FindNearLinkedScans结果之中

1.5 AddRunningScan

runningScan的长度通常是20m，这个长度指的是头和尾的距离，如果发现头尾长度大于20m，就去掉头。在开始时长度没有那么长，所以会添加，添加的数量有限，数量 = 20 m/ 采样的距离

1.6 TryLoopClosure

闭环检测使用 MatchScan来进行，会去除掉相邻帧，这是因为这些已经优化过了，利用其他的非相邻的并且 barycenter距离小于4m的帧进行试探性的闭环，其做法是首先寻找 这样的帧，然后寻找该帧前后的帧是否同样满足 小于4m的条件，如果超过了一定数量，就保留这个链， 可能有多个链被保留。 使用这个链代替 runningScan去做MatchScan， 判断response，如果response足够大，就进行闭环优化。
LinkChainToScan(candidateChain, pScan, bestPose, covariance);