cartographer 代码思想解读（14）- 后端优化核心类PoseGraph2D简介

最新推荐文章于 2024-07-03 11:31:18 发布

jiajiading

最新推荐文章于 2024-07-03 11:31:18 发布

阅读量1.4k

点赞数

分类专栏： slam

本文链接：https://blog.csdn.net/jiajiading/article/details/109994306

版权

slam 专栏收录该内容

29 篇文章 89 订阅

订阅专栏

cartographer 代码思想解读（14）- 后端优化核心类PoseGraph2D简介

PoseGraph2D成员
- submap新封装InternalSubmapData
- 约束类constraints定义
ConstraintBuilder2D约束构造器
OptimizationProblem2D优化器

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两种标志表示。