1、后端 Global SLAM(全局优化与回环)
后端基于 Konolige 等人(IROS 2010)的SPA(Sparse Pose Adjustment)回环公式并扩展到子图。
每个节点都与一个或多个子图匹配产生约束,节点位姿与子图位姿联合优化。
PoseGraph 是全局优化与回环的关键类;
继承关系如下:
PoseGraphInterface → PoseGraph → PoseGraph2D
1.1 PoseGraph 与工作队列
存储:所有共享后端状态集中在PoseGraphData data_(pose_graph_data.h),受mutex_保护:子图数据、全局子图位姿、所有轨迹节点、路标节点、轨迹连通性、轨迹状态、初始位姿、约束集合。
后台执行模型(work_queue.h):
AddWorkItem():若队列空则创建队列并在线程池调度DrainWorkQueue(),再入队工作项;