cartographer 代码思想解读(14)- 后端优化核心类PoseGraph2D简介
Mapbuilder中引入了整个slam分为前端和后端两部分,其中后端核心接口为PoseGraph
,并且分为2d和3d两种情况,2d声明接口为PoseGraph2D。
global_trajectory_builder中引入了后端PoseGraph2D核心类接口的调用,即前端匹配的后的结果类matching_result
加入到后端AddNode
接口。
PoseGraph2D成员
const proto::PoseGraphOptions options_; // 配置信息
GlobalSlamOptimizationCallback global_slam_optimization_callback_; // 回调,传出优化结果
mutable absl::Mutex mutex_; // 锁
absl::Mutex work_queue_mutex_; // 处理缓存数据锁
// If it exists, further work items must be added to this queue, and will be
// considered later.
std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(work_queue_mutex_); // 相当于一个缓存buffer,将要处理的数据缓存
// We globally localize a fraction of the nodes from each trajectory.
absl::flat_hash_map<int, std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_);
// Number of nodes added since last loop closure.
int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; // 自从上次闭环处理后增加的node的个数
// Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_; // 优化器
constraints::ConstraintBuilder2D constraint_builder_; // 约束构造器
// Thread pool used for handling the work queue.
common::ThreadPool* const thread_pool_; // 线程池,可处理work queue
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);
PoseGraphData data_ GUARDED_BY(mutex_); // PoseGraphData全局数据
ValueConversionTables conversion_tables_; // value变换查询表
其中大部分为全局内部成员的构建结构函数,其中PoseGraphData data_
为后端处理的全局数据,其详细成员为
struct PoseGraphData {
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
MapById<SubmapId, InternalSubmapData> submap_data; // submap 对应的submapID集合
// Global submap poses currently used for displaying data.
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d; // submap的全局位置
MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;
// Data that are currently being shown.
MapById<NodeId, TrajectoryNode> trajectory_nodes; // scanID 与trajectory对应集合
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
landmark_nodes;
// How our various trajectories are related.
TrajectoryConnectivityState trajectory_connectivity_state; // 轨迹之间的状态
int num_trajectory_nodes = 0;
std::map<int, InternalTrajectoryState> trajectories_state; // 轨迹状态集合
// Set of all initial trajectory poses.
std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses; // 每个trajectory的初始位置
std::vector<PoseGraphInterface::Constraint> constraints; // 约束队列
};
成员中的submap_data则为前端输出的子图,而trajectory_nodes为所有节点对象,constraints则为节点与子图之间的相对位姿,即所谓的约束。这三部分则构成了上节介绍的优化的位姿态图结构基本元素。其中trajectory_nodes数据格式在LocalTrajectoryBuilder2D类已经分析过,主要存储了每个node节点的激光和匹配结果信息包括位置,重力加速度方向,修正后的水平点云数据等。
submap新封装InternalSubmapData
// graph 中存储的submap格式
struct InternalSubmapData {
std::shared_ptr<const Submap> submap;
SubmapState state = SubmapState::kNoConstraintSearch;
// IDs of the nodes that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'kFinished'.
// 插入该地图的scan ID集合,在此submap称为kFinished时无需进行match
std::set<NodeId> node_ids;
};
在后端posegraph类中的submap增加了nodeid和state两个信息,其中node_id则为插入到本submap中的所有的nodeid集合,可用于区别node节点是否为本submap的内部节点;而state则表示当前submap是否更新完成。之所以存在state,是因为如果当前submap完成更新时,则会被要求与之前所有的节点进行匹配,则可以认为是一个闭环操作。
约束类constraints定义
上节简单讲解了cartographer后端优化的基本原理,引入了位姿图的概念,位姿图之间的关系称为约束,即节点之间的关系或者节点与submap的相对关系等。
struct Constraint {
struct Pose {
transform::Rigid3d zbar_ij; // 两个graph node的相对位姿
double translation_weight; // 平移权重
double rotation_weight; // 旋转权重
};
SubmapId submap_id; // 'i' in the paper. submap对应index的信息,同时指示trajectory id
NodeId node_id; // 'j' in the paper. scan对应index的信息,同时指示trajectory id
// Pose of the node 'j' relative to submap 'i'.
Pose pose; // scan节点对应在submap中的位置
// Differentiates between intra-submap (where node 'j' was inserted into
// submap 'i') and inter-submap constraints (where node 'j' was not inserted
// into submap 'i').
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; // 表示当前节点scan对应的submap是否为插入的
};
其中的SubmapId和NodeId则为记录在整个slam轨迹中唯一的submap和node ID号。由于cartographer支持多条轨迹,前面章节已经提过,用于理解可简单认为trajectory仅有一条即可。(由于可能多条,故SubmapId和NodeId中也包含了trajectory id的信息)
cartographer中的约束仅是每个节点与submap的相对关系,实际上节点间也存在,但是有一定的重复。详看上面的定义,其中pose则为node j在submap i中的位置,即记录了对应node和submap之间的相对位姿。
约束也被区分为是否为构建对应的submap的成员,从而区分中约束为前端匹配约束还是闭环约束,其状态由INTRA_SUBMAP
和INTER_SUBMAP
两种标志表示。
ConstraintBuilder2D约束构造器
cartographer优化的位姿图中节点为submap和轨迹节点则有前端匹配构成,而之间约束则需要约束构造器完成。每增加一个新的轨迹节点时则需要增加约束,每完成一个submap创建时同样也需要增加约束。cartographer则采用了一个专门的构造器进行计算约束并维护。后面文章将详解讲解。
OptimizationProblem2D优化器
有了位姿图和约束关系,即可创建超定方程组,可采用优化方法进行求解,而cartographer采用了SPA的优化方案进行优化求解,求得每个节点和submap最佳全局位姿使得超定方程组误差最小,即完成SLAM,后面将单独详细分析源码。