cartographer_ pose_ graph_ 2d

0. introduction

This paper mainly studies the pose_Graph Part of the logic in , In particular, two kinds of constraint calculation in back-end optimization .

Nonstandard class diagram ：

Some data structures ：

The concept of nodes and constraints

• node : A.tracking_frame The position of ; B. The pose of the origin of the subgraph
• constraint : tracking_frame Coordinate transformation with the origin of the subgraph

1.PoseGraph2D::AddNode

1.1.GetLocalToGlobalTransform

GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose

• $T_{gl}\ast T_{ln}=T_{gn}$ ： Place nodes in local The pose in the coordinate system is transformed into global The pose in the coordinate system
• $T_{gl}$ ：GetLocalToGlobalTransform(trajectory_id)
• $T_{ln}$：node stay local map frame The pose in the coordinate system

This step is mainly to node Add to PoseGraph2D Of PoseGraphData in .

  //  Add a new node to the node list 
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{
    constant_data, optimized_pose});

The node pose added here , In fact, we can see from the front-end results , There is another meaning here , It includes submap The position of ：

1.2.AppendNode

The front end only maintains two submap, The rest are transferred to the back end for storage ：

  // Test if the 'insertion_submap.back()' is one we never saw before.
  //  If it is the beginning of the track ,  perhaps insertion_submaps.back() It's the first time I've seen ,  Add a new subgraph 
  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
              ->data.submap != insertion_submaps.back()) {
    
    // We grow 'data_.submap_data' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.

    //  If insertion_submaps.back() It's the first time I've seen ,  That is, the newly generated 
    //  stay data_.submap_data Add an empty InternalSubmapData
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());
    
    //  Save the map behind ,  Assign the pointer of the following map to the past 
    //  The map is newly generated ,  But the map will be updated in the front part by inserting point cloud data ,  Only the pointer is saved here 
    // tag:  Draw a picture to illustrate 
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }

Maps, which have a large amount of data, are pointers , Open up memory only once ,cartographer Memory management is very good .

submap Only the front end will change , The back end will not , So in that case , The front-end map without optimized pose is not accurate , Where is the map optimized ？

1.3.AddWorkItem

stay Add a subgraph trimmer AddTrimmer AddImuData AddOdometryData AddFixedFramePoseData AddLandmarkData And other external interface functions （Global Call in the ） This function is called in .

AddWorkItem() This in the argument lambda The expression does not actually execute immediately , Instead, it is added to the task scheduling .

• work_queue_ Task scheduling
• thread_pool_ Thread pool

TODO： To learn .

stay PoseGraph2D::AddNode After joining the thread pool , It's just DrainWorkQueue in Thread pool Execute in a loop .

2.ComputeConstraintsForNode Constraint calculation in subgraph

• 1. obtain node Of constant_data:

• 2. obtain trajectory_id Of a subgraph that is in an active state SubmapId : InitializeGlobalSubmapPoses
  A).data_.global_submap_poses_2d: All the optimized subgraphs are in global In a coordinate system pose
  B).optimization_problem_->submap_data(): Including optimized and not yet optimized Subgraph in global In a coordinate system pose
  C).ComputeLocalToGlobalTransform() Parameters of this function , It's always data_.global_submap_poses_2d, The calculation is optimized global Point to local Coordinate transformation of .

constraint ：

  //  Containing subgraphs id,  Node id,  node j Relative to subgraph i Coordinate transformation of ,  And whether the node is in or out of the subgraph 
  struct Constraint {
    
    struct Pose {
    
      transform::Rigid3d zbar_ij;
      double translation_weight;
      double rotation_weight;
    };

    SubmapId submap_id;  // 'i' in the paper.
    NodeId node_id;      // 'j' in the paper.

    // Pose of the node 'j' relative to submap 'i'.
    Pose pose;

    // 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;
  };

Figure since

cartographer Back end constraint type ：

3.ComputeConstraints Constraint calculation between subgraphs （ Loop back detection ）

The constraint between two subgraphs calls the same function , All are node <--> submap. among for One step in the cycle ：

• Nodes are matched to a part of the subgraph ConstraintBuilder2D::MaybeAddConstraint ( Only this... Will be executed during drawing creation , Loop detection through local search )

• Nodes are matched with all subgraphs ConstraintBuilder2D::MaybeAddGlobalConstraint ( It is possible to perform this operation only when positioning )

The main logic flows to （ One node With a submap Match ）：

/** * @brief  Compute a constraint between nodes and subgraphs ( Loop back detection ) *  Rough matching with a matcher based on branch and bound algorithm , And then use ceres Perform fine matching  * * @param[in] submap_id submap Of id * @param[in] submap  Map data  * @param[in] node_id  node id * @param[in] match_full_submap  Is it local matching or full subgraph matching  * @param[in] constant_data  Node data  * @param[in] initial_relative_pose  The initial value of the constraint  * @param[in] submap_scan_matcher  matcher  * @param[out] constraint  Calculated constraints  */
void ConstraintBuilder2D::ComputeConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, bool match_full_submap,
    const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose,
    const SubmapScanMatcher& submap_scan_matcher,
    std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
    

• Step:1 Get the node at local frame The coordinates under
• Step:2 Use the matcher based on branch and bound algorithm for rough matching （ Innovation of the paper , Study by writing alone ）, If the score is too low , No more fine matching , Go straight back to
• Step:3 Use ceres Perform fine matching , It is the function used for front-end scan matching
• Step:4 Get nodes to submap Coordinate transformation between the origin of the coordinate system
• Step:5 Returns the calculated constraint

Loop back detection consists of two modes ：

• // param: global_localization_min_score The lowest score threshold for loop detection of a whole subgraph 0.6 （ Pure positioning ）
• // param: min_score The lowest score threshold for loop detection of local subgraphs 0.55