cartographer_ optimization_ problem_ 2d

0. introduction

The main business logic goes back to mapping/internal/2d/pose_graph_2d.cc/PoseGraph2D::HandleWorkQueue-->RunOptimization() in . The specific optimization part is in optimization Implementation in .

The previous calculation constraint result is ：

1. Set variables to be optimized

• Of subgraphs global_pose
• Node global_pose_2d

2. Residuals

2.1. The first residual -scan_match

take The node and the origin of the subgraph are global Relative position and orientation in coordinate system And constraint As the residual term

• The first coordinate transformation : The node and the origin of the subgraph are global Coordinate transformation in coordinate system （ Predict pose ）
• The second coordinate transformation : Constraints within and between subgraphs （ Measuring posture ）

Residual calculation code ：

/** * @brief 2d  according to SPA The formula in the paper calculates the residual error  * *  Calculated residual error ： * T12 = T1.inverse() * T2 * [R1.inverse * R2, R1.inverse * (t2 -t1)] * [0 , 1 ] * * @param[in] relative_pose * @param[in] start * @param[in] end * @return std::array<T, 3> */
template <typename T>
static std::array<T, 3> ComputeUnscaledError(
    const transform::Rigid2d& relative_pose, const T* const start,
    const T* const end) {
    
  //  Rotation matrix R
  const T cos_theta_i = cos(start[2]);
  const T sin_theta_i = sin(start[2]);
  const T delta_x = end[0] - start[0]; // t2 -t1
  const T delta_y = end[1] - start[1];
  const T h[3] = {
    cos_theta_i * delta_x + sin_theta_i * delta_y, // R.inverse * (t2 -t1)
                  -sin_theta_i * delta_x + cos_theta_i * delta_y,
                  end[2] - start[2]};
  return {
    {
    T(relative_pose.translation().x()) - h[0],
           T(relative_pose.translation().y()) - h[1],
           common::NormalizeAngleDifference(
               T(relative_pose.rotation().angle()) - h[2])}};
}

2.2. The second residual -Landmark

landmark data And adopt 2 The relative pose interpolated by the pose of nodes As the residual term

• The first coordinate transformation : landmark The time of data is 2 The pose interpolated from the pose of nodes （ Predict pose ）
• The second coordinate transformation : landmark In the data landmark_to_tracking_transform_（ Measuring posture ）

  struct LandmarkNode {
    
    // landmark Data is relative to tracking_frame Relative coordinate transformation of 
    struct LandmarkObservation {
    
      int trajectory_id;
      common::Time time;
      transform::Rigid3d landmark_to_tracking_transform;
      double translation_weight;
      double rotation_weight;
    };
    //  Multiple... May be observed at the same time landmark data 
    std::vector<LandmarkObservation> landmark_observations;
    //  This frame of data corresponds to tracking_frame stay global The pose in the coordinate system 
    absl::optional<transform::Rigid3d> global_landmark_pose;
    bool frozen = false;
  };

Mainly node <--> landmark Constraints between ：

  // Step: landmark data   And   adopt 2 The relative pose interpolated by the pose of nodes   As the residual term 
  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
                           &problem, options_.huber_scale());

A little .

2.3. The third residual -Odometer

Nodes are located between nodes global Relative coordinate transformation in coordinate system And Relative coordinate transformation interpolated from odometer data As the residual term

• The first coordinate transformation : adjacent 2 Nodes are located between global Coordinate transformation in coordinate system
• The second coordinate transformation : Then calculate these separately 2 The time of nodes is interpolated in the odometer data queue 2 Odometer position and attitude , Calculate this 2 Coordinate transformation between position and attitude of odometer

2.4. Fourth residual -local_global

Nodes are located between nodes global Relative coordinate transformation in coordinate system And adjacent 2 A node in the local Relative coordinate transformation in coordinate system As the residual term

• The first coordinate transformation : adjacent 2 There are three internodes in global Coordinate transformation in coordinate system
• The second coordinate transformation : adjacent 2 A node in the local Coordinate transformation in coordinate system

2.5. The fifth residual -gps

Node and gps The origin of the coordinate system is global Relative coordinate transformation in coordinate system And adopt gps The relative coordinate transformation obtained by data interpolation As the residual term

• The first coordinate transformation : The corresponding time of the node is gps Interpolated data gps be relative to gps The position and pose of the origin of the coordinate system
• The second coordinate transformation : Nodes in the global In coordinate system And gps Origin of coordinate system Relative coordinate transformation of

3. Theoretical reference

• Reference resources 1
• Reference resources 2