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

综述

本文旨在提供关于 open_karto 代码的详细中文解释，通过open_karto提供的tutorial1来详细分析代码含义。针对代码中重点类LocalizedRangeScan,LaserRangeScan,Mapper,ScanMatcher,CorrelationGrid,GridIndexLookup等进行详细解释，了解其作用。另外对重点函数Mapper::Process(), ScanMatcher::MatchScan(...),Mapper::Initialize(),MapperGraph::AddEdges()等进行详细说明

代码地址

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

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

其他相关博客

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

需要知道的知识

• 现实中laser放在车上，在代码中 laser又称为sensor，车又称为 robot

知识点梳理

类 概述

LaserRangeFinder

该类管理关于 laser的相关信息，包括:

• laser名字 : 在tutorial1中为 laser0
• laser的扫描范围:最大角度，最小角度等等
• laser与车中心之间的相对关系 : SetOffsetPose()实现，tutorial1中为karto::Pose2(1.0, 0.0, 0.0)，意为laser相对于车的坐标为 (1,0)，同时相对车的转向角（karto称之为Heading）为 0,
• laser 的分辨率，每两根laser射线之间的夹角，配合laser的范围，可以确定laser的射线数量laser 的分辨率，每两根laser射线之间的夹角，配合laser的范围，可以确定laser的射线数量
• laser 的 RangeThreshold，用于栅格的大小。

LaserRangeScan

• kt_double* m_pRangeReadings 从laser读取的原始数据
• kt_int32u m_NumberOfRangeReadings laser射线数量
• 提供一些获取数据以及获取数据来源(即来自于 laser0)的方法

LocalizedRangeScan : public LaserRangeScan

• PointVectorDouble m_PointReadings 将laser的扫描数据转换为 在世界坐标系中的二维坐标结果，在Update()函数中实现
• BoundingBox2 m_BoundingBox 将laser扫描数据在世界坐标系中的结果用boundingbox圈起来
• Pose2 m_OdometricPose 机器人的里程计结果，不是准确的结果，需要优化，注意不是laser位姿
• Pose2 m_CorrectedPose 通过karto算法纠正后的机器人的位姿，注意不是laser位姿
• Pose2 m_BarycenterPose 将 m_PointReadings的每一个点强行加起来算出的平均点位置
• Update() 将上述变量进行赋值的函数
• void SetSensorPose(const Pose2& rScanPose) 将karto算法得到的结果赋值的函数，较为复杂，可以看文中开头的代码网址中的详细介绍

ScanMatcher

• Mapper* m_pMapper 地图，见下面的地图释义
• CorrelationGrid* m_pCorrelationGrid 栅格地图，用于MatchScan函数计算response
• Grid<kt_double>* m_pSearchSpaceProbs 不太懂
• GridIndexLookup<kt_int8u>* m_pGridLookup 用来存储经过不同旋转之后的nPoints个扫描点应该落在的位置上面

Mapper

非常重要的类，可以认为管理了整个karto算法大部分内容的类

变量

• ScanMatcher* m_pSequentialScanMatcher 将上面的ScanMatcher类存储到 Mapper之中，主要发挥计算runningScans和当前scan数据之间的里程计的作用
• MapperSensorManager* m_pMapperSensorManager 这个也是非常重要的内容。用于管理历史 laserScans以及runningScans，其中runningScans是用来做匹配的
• MapperGraph* m_pGraph
• ScanSolver* m_pScanOptimizer 在karto算法中为SPA，一种类似于g2o的优化算法
• Parameter<kt_bool>* m_pUseScanMatching 众多预设参数的一个，可以去看网上关于 karto参数的理解的博客

函数方法

• Mapper::Initialize(kt_double rangeThreshold) 初始化了Mapper类内的各种重要变量，如ScanMatcher* m_pSequentialScanMatcher， MapperSensorManager* m_pMapperSensorManager等
• kt_bool Mapper::Process(LocalizedRangeScan* pScan) 每获取一帧新数据，就要进行karto算法处理。这个相当于Mapper的主函数
• kt_bool Mapper::HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) 判断当前帧是否可以进行karto算法，通过两帧之间的距离以及时间来判断
• void Mapper::FireInfo(const std::string& rInfo) const 用于输出一些信息的函数，其他Fire函数也是这样
• inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName) 试图找回环的函数，具体解释见文章开头代码地址里面的详细解释

MapperGraph

• Mapper* m_pMapper 与Mapper属于相互引用的关系，地址一样，所以功能一样

• ScanMatcher* m_pLoopScanMatcher

• GraphTraversal<LocalizedRangeScan>* m_pTraversal 利用深度优先算法(BFS)来搜索当前帧的邻近帧的算法

• void AddVertex(LocalizedRangeScan* pScan) 图优化中顶点

• Edge<LocalizedRangeScan>* AddEdge(LocalizedRangeScan* pSourceScan, LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge) 图优化中边的概念

• FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance) 利用深度优先算法搜索相邻的帧

• LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan, const Name& rSensorName, kt_int32u& rStartNum)
  找回环的一部分代码

自己整理的karto.h以及Mapper.h中的类的合集

karto.h

mapper.h

Mapper scanMatcher MapperGraph关系图