Improve pose terminology of 2D optimization problem. (#1053)

Removes the confusing term `initial_pose` from global 2D optimization.
It makes sense in other areas like scan matching, but has a completely
different meaning there. Also matches the 3D equivalents better.

- `local_pose_2d` replaces `initial_pose` as the name for the
   non-gravity-aligned, local 2D pose in the submap
- `global_pose_2d` replaces `pose` as the name for the
   non-gravity-aligned, global 2D pose that is optimized

See the comment in the PR for how this improves readability.

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc

@@ -231,7 +231,7 @@ void OptimizationProblem2D::Solve(
   for (const auto& node_id_data : node_data_) {
     const bool frozen =
         frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
-    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.pose));
+    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
     problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
     if (frozen) {
       problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
@@ -288,8 +288,8 @@ void OptimizationProblem2D::Solve(
 
       // Add a relative pose constraint based on consecutive local SLAM poses.
       const transform::Rigid3d relative_local_slam_pose =
-          transform::Embed3D(first_node_data.initial_pose.inverse() *
+          transform::Embed3D(first_node_data.local_pose_2d.inverse() *
-                             second_node_data.initial_pose);
+                             second_node_data.local_pose_2d);
       problem.AddResidualBlock(
           SpaCostFunction2D::CreateAutoDiffCostFunction(
               Constraint::Pose{relative_local_slam_pose,
@@ -315,7 +315,8 @@ void OptimizationProblem2D::Solve(
         ToPose(C_submap_id_data.data);
   }
   for (const auto& C_node_id_data : C_nodes) {
-    node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);
+    node_data_.at(C_node_id_data.id).global_pose_2d =
+        ToPose(C_node_id_data.data);
   }
   for (const auto& C_landmark : C_landmarks) {
     landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h

@@ -42,8 +42,8 @@ namespace pose_graph {
 
 struct NodeData2D {
   common::Time time;
-  transform::Rigid2d initial_pose;
+  transform::Rigid2d local_pose_2d;
-  transform::Rigid2d pose;
+  transform::Rigid2d global_pose_2d;
   Eigen::Quaterniond gravity_alignment;
 };
 

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -209,7 +209,7 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
         optimization_problem_->submap_data()
             .at(submap_id)
             .global_pose.inverse() *
-        optimization_problem_->node_data().at(node_id).pose;
+        optimization_problem_->node_data().at(node_id).global_pose_2d;
     constraint_builder_.MaybeAddConstraint(
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         trajectory_nodes_.at(node_id).constant_data.get(),
@@ -240,16 +240,16 @@ void PoseGraph2D::ComputeConstraintsForNode(
       node_id.trajectory_id, constant_data->time, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const SubmapId matching_id = submap_ids.front();
-  const transform::Rigid2d pose = transform::Project2D(
+  const transform::Rigid2d local_pose_2d = transform::Project2D(
       constant_data->local_pose *
       transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
-  const transform::Rigid2d optimized_pose =
+  const transform::Rigid2d global_pose_2d =
       optimization_problem_->submap_data().at(matching_id).global_pose *
       pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
-      pose;
+      local_pose_2d;
   optimization_problem_->AddTrajectoryNode(
       matching_id.trajectory_id,
-      pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
+      pose_graph::NodeData2D{constant_data->time, local_pose_2d, global_pose_2d,
                              constant_data->gravity_alignment});
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const SubmapId submap_id = submap_ids[i];
@@ -258,7 +258,8 @@ void PoseGraph2D::ComputeConstraintsForNode(
     CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
     submap_data_.at(submap_id).node_ids.emplace(node_id);
     const transform::Rigid2d constraint_transform =
-        pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose;
+        pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
+        local_pose_2d;
     constraints_.push_back(Constraint{submap_id,
                                       node_id,
                                       {transform::Embed3D(constraint_transform),
@@ -567,7 +568,7 @@ void PoseGraph2D::RunOptimization() {
     for (const auto& node : node_data.trajectory(trajectory_id)) {
       auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
       mutable_trajectory_node.global_pose =
-          transform::Embed3D(node.data.pose) *
+          transform::Embed3D(node.data.global_pose_2d) *
           transform::Rigid3d::Rotation(
               mutable_trajectory_node.constant_data->gravity_alignment);
     }