(一)PoseGraph2D类
位姿图Node的插入
在GlobalTrajectoryBuilder
类中调用AddSensorData()
接口更新激光点云数据时,会调用PoseGraph2D
类的AddNode()
接口。AddNode()
函数中会调用AppendNode()
函数,然后调用ComputeConstraintsForNode()
函数计算约束。
约束计算和回环检测
工作流处理机制
AddWorkItem()
HandleWorkQueue()
DrainWorkQueue()
(二)MapBuilder类
删除submap机制
共包含两种trim的机制,第一种是OverlappingSubmapsTrimmer2D
类型的,只用在2D SLAM中;第二种是PureLocalizationTrimmer
,在2D和3D中都可以用。
在MapBuilder
类中调用AddTrajectoryBuilder()
接口时,会调用PoseGraph2D
类的AddTrimmer()
接口,向pose_graph中配置PoseGraphTrimmer
类。
在PoseGraph2D
类调用HandleWorkQueue()
函数处理工作流的过程中,会调用Trim()
函数执行submap的筛选和删除操作。MapBuilder
类中一般会向pose_graph中配置PureLocalizationTrimmer
类,在纯定位的情况下会只保留3张submap,每次有新的submap进来的时候会删除最早的一张submap,具体实现细节在PureLocalizationTrimmer::Trim()
函数中。OverlappingSubmapsTrimmer2D
类
(三)LocalTrajectoryBuilder2D类
管理外推器PoseExtrapolator
InitializeExtrapolator()
函数:初始化extrapolator_
指针,然后调用PoseExtrapolator::AddPose()
函数添加初始位姿。AddImuData()
函数:调用PoseExtrapolator::AddImuData()
添加IMU数据,初次调用时会首先使用InitializeExtrapolator()
函数初始化外推器.AddOdometryData()
函数:调用PoseExtrapolator::AddOdometryData()
添加轮式里程计数据。
激光点云数据处理
通过AddRangeData()
接口传入激光点云数据,
调用AddAccumulatedRangeData()
函数
在AddAccumulatedRangeData()
函数中,调用sensor::AdaptiveVoxelFilter()
函数对点云数据进行自适应体素滤波,然后调用ScanMatch()
函数得到点云匹配位姿,然后调用PoseExtrapolator::AddPose()
函数将匹配位姿添加到外推器,然后调用InsertIntoSubmap()
函数将激光点云数据插入到submap
激光点云去畸变
ScanMatch帧间匹配
Submap的生成及数据插入
通过调用InsertIntoSubmap()
函数将激光点云数据插入到submap中,其中首先要调用MotionFilter
类的IsSimilar()
函数控制插入的激光点云帧数稀疏化
(四)GlobalTrajectoryBuilder类
(五)PoseExtrapolator类,外推器
位姿外插器的主要作用是利用保存了一段时间的位姿计算角速度和线速度,并利用线速度和角速度进行位姿插值。
AddPose()
AddImuData()
AddOdometryData()
ExtrapolatePose()
ExtrapolateTranslation()
ExtrapolateRotation()
AdvanceImuTracker()
(六)MotionFilter类,前端Node稀疏化
在trajectory_builder_2d.cc
的LocalTrajectoryBuilder2D::AddAccumulatedRangeData()
函数中执行稀疏化任务,即在AddAccumulatedRangeData()
函数中调用InsertIntoSubmap()
函数,在InsertIntoSubmap()
函数内调用MotionFilter::IsSimilar()
函数进行稀疏化,如果被判断和上一帧的时间及位姿变化较小,则认为该帧不是Node,不放入后端。