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

最新推荐文章于 2024-06-19 09:33:42 发布

置顶 wphkadn

最新推荐文章于 2024-06-19 09:33:42 发布

阅读量2.3k

点赞数 1

分类专栏： c++ karto SLAM 文章标签： SLAM

本文链接：https://blog.youkuaiyun.com/wphkadn/article/details/90247146

版权

c++ 同时被 3 个专栏收录

30 篇文章

订阅专栏

SLAM

9 篇文章

订阅专栏

karto

2 篇文章

订阅专栏

前言

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中。
该函数的作用是处理当前激光数据帧，包含的主要过程有：

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

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

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

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

将当前帧加入数据库中
将当前帧设置为图优化的顶点
m_pGraph->AddVertex(pScan);
将当前帧执行添加边的操作，边和顶点是图优化的重要概念，可参考SLAM十四讲
m_pGraph->AddEdges(pScan, covariance);
将当前帧添加到runningScan中，并进行维护. runningScan即是第5步中被比较的对象，也即是“前面的帧”
m_pMapperSensorManager->AddRunningScan(pScan);
闭环检测
m_pGraph->TryCloseLoop(pScan, *iter);
将当前帧设置为 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 如何判定相邻？

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

1.4.2 怎么找？`FindNearChains`

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

1.4.3 添加边的过程

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

1.4.4 备注

AddEdges并不是都要调用 LinkNearChains来添加，也可能直接调用LinkScans来添加边。
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);