综述
本文旨在提供关于 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函数计算responseGrid<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)
找回环的一部分代码