Use PoseGraphData in PoseGraph3d. (#1188)

[RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0022-pose-graph-data.md)

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -61,26 +61,28 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
   if (insertion_submaps.size() == 1) {
     // If we don't already have an entry for the first submap, add one.
     if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
-      if (initial_trajectory_poses_.count(trajectory_id) > 0) {
+      if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
-        trajectory_connectivity_state_.Connect(
+        data_.trajectory_connectivity_state.Connect(
             trajectory_id,
-            initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
+            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
+            time);
       }
       optimization_problem_->AddSubmap(
-          trajectory_id,
+          trajectory_id, ComputeLocalToGlobalTransform(
-          ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) *
+                             data_.global_submap_poses_3d, trajectory_id) *
-              insertion_submaps[0]->local_pose());
+                             insertion_submaps[0]->local_pose());
     }
     CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
     const SubmapId submap_id{trajectory_id, 0};
-    CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
+    CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
     return {submap_id};
   }
   CHECK_EQ(2, insertion_submaps.size());
   const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
   CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
   const SubmapId last_submap_id = std::prev(end_it)->id;
-  if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
+  if (data_.submap_data.at(last_submap_id).submap ==
+      insertion_submaps.front()) {
     // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
     // and 'insertions_submaps.back()' is new.
     const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
@@ -91,10 +93,12 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
     return {last_submap_id,
             SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
   }
-  CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());
+  CHECK(data_.submap_data.at(last_submap_id).submap ==
+        insertion_submaps.back());
   const SubmapId front_submap_id{trajectory_id,
                                  last_submap_id.submap_index - 1};
-  CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());
+  CHECK(data_.submap_data.at(front_submap_id).submap ==
+        insertion_submaps.front());
   return {front_submap_id, last_submap_id};
 }
 
@@ -107,19 +111,19 @@ NodeId PoseGraph3D::AddNode(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(trajectory_id);
-  const NodeId node_id = trajectory_nodes_.Append(
+  const NodeId node_id = data_.trajectory_nodes.Append(
       trajectory_id, TrajectoryNode{constant_data, optimized_pose});
-  ++num_trajectory_nodes_;
+  ++data_.num_trajectory_nodes;
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
-  if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
+  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
-      std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
+      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
-          insertion_submaps.back()) {
+              ->data.submap != insertion_submaps.back()) {
-    // We grow 'submap_data_' as needed. This code assumes that the first
+    // We grow 'data_.submap_data' as needed. This code assumes that the first
     // time we see a new submap is as 'insertion_submaps.back()'.
     const SubmapId submap_id =
-        submap_data_.Append(trajectory_id, InternalSubmapData());
+        data_.submap_data.Append(trajectory_id, InternalSubmapData());
-    submap_data_.at(submap_id).submap = insertion_submaps.back();
+    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
   }
 
   // We have to check this here, because it might have changed by the time we
@@ -141,7 +145,7 @@ void PoseGraph3D::AddWorkItem(const std::function<void()>& work_item) {
 }
 
 void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
-  trajectory_connectivity_state_.Add(trajectory_id);
+  data_.trajectory_connectivity_state.Add(trajectory_id);
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {
     global_localization_samplers_[trajectory_id] =
@@ -182,7 +186,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
   common::MutexLocker locker(&mutex_);
   AddWorkItem([=]() REQUIRES(mutex_) {
     for (const auto& observation : landmark_data.landmark_observations) {
-      landmark_nodes_[observation.id].landmark_observations.emplace_back(
+      data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
           PoseGraphInterface::LandmarkNode::LandmarkObservation{
               trajectory_id, landmark_data.time,
               observation.landmark_to_tracking_transform,
@@ -193,7 +197,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
 
 void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
                                     const SubmapId& submap_id) {
-  CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
+  CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
 
   const transform::Rigid3d global_node_pose =
       optimization_problem_->node_data().at(node_id).global_pose;
@@ -205,16 +209,17 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
       global_submap_pose.inverse();
 
   std::vector<TrajectoryNode> submap_nodes;
-  for (const NodeId& submap_node_id : submap_data_.at(submap_id).node_ids) {
+  for (const NodeId& submap_node_id :
-    submap_nodes.push_back(
+       data_.submap_data.at(submap_id).node_ids) {
-        TrajectoryNode{trajectory_nodes_.at(submap_node_id).constant_data,
+    submap_nodes.push_back(TrajectoryNode{
-                       global_submap_pose_inverse *
+        data_.trajectory_nodes.at(submap_node_id).constant_data,
-                           trajectory_nodes_.at(submap_node_id).global_pose});
+        global_submap_pose_inverse *
+            data_.trajectory_nodes.at(submap_node_id).global_pose});
   }
 
   const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
   const common::Time last_connection_time =
-      trajectory_connectivity_state_.LastConnectionTime(
+      data_.trajectory_connectivity_state.LastConnectionTime(
           node_id.trajectory_id, submap_id.trajectory_id);
   if (node_id.trajectory_id == submap_id.trajectory_id ||
       node_time <
@@ -226,9 +231,11 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
     // submap's trajectory, it suffices to do a match constrained to a local
     // search window.
     constraint_builder_.MaybeAddConstraint(
-        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
+        submap_id,
-        trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
+        static_cast<const Submap3D*>(
-        global_node_pose, global_submap_pose);
+            data_.submap_data.at(submap_id).submap.get()),
+        node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
+        submap_nodes, global_node_pose, global_submap_pose);
   } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
     // In this situation, 'global_node_pose' and 'global_submap_pose' have
     // orientations agreeing on gravity. Their relationship regarding yaw is
@@ -236,14 +243,17 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
     // matching procedure in the FastCorrelativeScanMatcher, and the given yaw
     // is essentially ignored.
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
+        submap_id,
-        trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
+        static_cast<const Submap3D*>(
-        global_node_pose.rotation(), global_submap_pose.rotation());
+            data_.submap_data.at(submap_id).submap.get()),
+        node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
+        submap_nodes, global_node_pose.rotation(),
+        global_submap_pose.rotation());
   }
 }
 
 void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
-  const auto& submap_data = submap_data_.at(submap_id);
+  const auto& submap_data = data_.submap_data.at(submap_id);
   for (const auto& node_id_data : optimization_problem_->node_data()) {
     const NodeId& node_id = node_id_data.id;
     if (submap_data.node_ids.count(node_id) == 0) {
@@ -256,7 +266,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
     const NodeId& node_id,
     std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
     const bool newly_finished_submap) {
-  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
+  const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
   const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
       node_id.trajectory_id, constant_data->time, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
@@ -271,12 +281,12 @@ void PoseGraph3D::ComputeConstraintsForNode(
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const SubmapId submap_id = submap_ids[i];
     // Even if this was the last node added to 'submap_id', the submap will only
-    // be marked as finished in 'submap_data_' further below.
+    // be marked as finished in 'data_.submap_data' further below.
-    CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
+    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
-    submap_data_.at(submap_id).node_ids.emplace(node_id);
+    data_.submap_data.at(submap_id).node_ids.emplace(node_id);
     const transform::Rigid3d constraint_transform =
         insertion_submaps[i]->local_pose().inverse() * local_pose;
-    constraints_.push_back(
+    data_.constraints.push_back(
         Constraint{submap_id,
                    node_id,
                    {constraint_transform, options_.matcher_translation_weight(),
@@ -284,7 +294,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
                    Constraint::INTRA_SUBMAP});
   }
 
-  for (const auto& submap_id_data : submap_data_) {
+  for (const auto& submap_id_data : data_.submap_data) {
     if (submap_id_data.data.state == SubmapState::kFinished) {
       CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
       ComputeConstraint(node_id, submap_id_data.id);
@@ -294,7 +304,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
   if (newly_finished_submap) {
     const SubmapId finished_submap_id = submap_ids.front();
     InternalSubmapData& finished_submap_data =
-        submap_data_.at(finished_submap_id);
+        data_.submap_data.at(finished_submap_id);
     CHECK(finished_submap_data.state == SubmapState::kActive);
     finished_submap_data.state = SubmapState::kFinished;
     // We have a new completed submap, so we look into adding constraints for
@@ -322,13 +332,14 @@ void PoseGraph3D::DispatchOptimization() {
 
 common::Time PoseGraph3D::GetLatestNodeTime(const NodeId& node_id,
                                             const SubmapId& submap_id) const {
-  common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
+  common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
-  const InternalSubmapData& submap_data = submap_data_.at(submap_id);
+  const InternalSubmapData& submap_data = data_.submap_data.at(submap_id);
   if (!submap_data.node_ids.empty()) {
     const NodeId last_submap_node_id =
-        *submap_data_.at(submap_id).node_ids.rbegin();
+        *data_.submap_data.at(submap_id).node_ids.rbegin();
     time = std::max(
-        time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
+        time,
+        data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
   }
   return time;
 }
@@ -337,16 +348,17 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
   CHECK_EQ(constraint.tag, PoseGraphInterface::Constraint::INTER_SUBMAP);
   const common::Time time =
       GetLatestNodeTime(constraint.node_id, constraint.submap_id);
-  trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
+  data_.trajectory_connectivity_state.Connect(
-                                         constraint.submap_id.trajectory_id,
+      constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id,
-                                         time);
+      time);
 }
 
 void PoseGraph3D::HandleWorkQueue(
     const constraints::ConstraintBuilder3D::Result& result) {
   {
     common::MutexLocker locker(&mutex_);
-    constraints_.insert(constraints_.end(), result.begin(), result.end());
+    data_.constraints.insert(data_.constraints.end(), result.begin(),
+                             result.end());
   }
   RunOptimization();
 
@@ -408,18 +420,19 @@ void PoseGraph3D::WaitForAllComputations() {
   while (!locker.AwaitWithTimeout(
       [this]() REQUIRES(mutex_) {
         return ((constraint_builder_.GetNumFinishedNodes() ==
-                 num_trajectory_nodes_) &&
+                 data_.num_trajectory_nodes) &&
                 !work_queue_);
       },
       common::FromSeconds(1.))) {
     // Log progress on nodes only when we are actually processing nodes.
-    if (num_trajectory_nodes_ != num_finished_nodes_at_start) {
+    if (data_.num_trajectory_nodes != num_finished_nodes_at_start) {
       std::ostringstream progress_info;
       progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
                     << 100. *
                            (constraint_builder_.GetNumFinishedNodes() -
                             num_finished_nodes_at_start) /
-                           (num_trajectory_nodes_ - num_finished_nodes_at_start)
+                           (data_.num_trajectory_nodes -
+                            num_finished_nodes_at_start)
                     << "%...";
       std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
     }
@@ -429,7 +442,8 @@ void PoseGraph3D::WaitForAllComputations() {
       [this,
        &notification](const constraints::ConstraintBuilder3D::Result& result) {
         common::MutexLocker locker(&mutex_);
-        constraints_.insert(constraints_.end(), result.begin(), result.end());
+        data_.constraints.insert(data_.constraints.end(), result.begin(),
+                                 result.end());
         notification = true;
       });
   locker.Await([&notification]() { return notification; });
@@ -438,11 +452,11 @@ void PoseGraph3D::WaitForAllComputations() {
 void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
-    CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
+    CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0);
-    finished_trajectories_.insert(trajectory_id);
+    data_.finished_trajectories.insert(trajectory_id);
 
-    for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
+    for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
-      submap_data_.at(submap.id).state = SubmapState::kFinished;
+      data_.submap_data.at(submap.id).state = SubmapState::kFinished;
     }
     CHECK(!run_loop_closure_);
     DispatchOptimization();
@@ -450,20 +464,20 @@ void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
 }
 
 bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
-  return finished_trajectories_.count(trajectory_id) > 0;
+  return data_.finished_trajectories.count(trajectory_id) > 0;
 }
 
 void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
-  trajectory_connectivity_state_.Add(trajectory_id);
+  data_.trajectory_connectivity_state.Add(trajectory_id);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
-    CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
+    CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0);
-    frozen_trajectories_.insert(trajectory_id);
+    data_.frozen_trajectories.insert(trajectory_id);
   });
 }
 
 bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const {
-  return frozen_trajectories_.count(trajectory_id) > 0;
+  return data_.frozen_trajectories.count(trajectory_id) > 0;
 }
 
 void PoseGraph3D::AddSubmapFromProto(
@@ -479,13 +493,13 @@ void PoseGraph3D::AddSubmapFromProto(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(submap_id.trajectory_id);
-  submap_data_.Insert(submap_id, InternalSubmapData());
+  data_.submap_data.Insert(submap_id, InternalSubmapData());
-  submap_data_.at(submap_id).submap = submap_ptr;
+  data_.submap_data.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
-  global_submap_poses_.Insert(submap_id,
+  data_.global_submap_poses_3d.Insert(
-                              optimization::SubmapSpec3D{global_submap_pose});
+      submap_id, optimization::SubmapSpec3D{global_submap_pose});
   AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
-    submap_data_.at(submap_id).state = SubmapState::kFinished;
+    data_.submap_data.at(submap_id).state = SubmapState::kFinished;
     optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
   });
 }
@@ -499,10 +513,12 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(node_id.trajectory_id);
-  trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose});
+  data_.trajectory_nodes.Insert(node_id,
+                                TrajectoryNode{constant_data, global_pose});
 
   AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
-    const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
+    const auto& constant_data =
+        data_.trajectory_nodes.at(node_id).constant_data;
     optimization_problem_->InsertTrajectoryNode(
         node_id,
         optimization::NodeSpec3D{constant_data->time, constant_data->local_pose,
@@ -533,7 +549,7 @@ void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id,
                                   const SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
-    submap_data_.at(submap_id).node_ids.insert(node_id);
+    data_.submap_data.at(submap_id).node_ids.insert(node_id);
   });
 }
 
@@ -542,13 +558,14 @@ void PoseGraph3D::AddSerializedConstraints(
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, constraints]() REQUIRES(mutex_) {
     for (const auto& constraint : constraints) {
-      CHECK(trajectory_nodes_.Contains(constraint.node_id));
+      CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
-      CHECK(submap_data_.Contains(constraint.submap_id));
+      CHECK(data_.submap_data.Contains(constraint.submap_id));
-      CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
+      CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data !=
-      CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
+            nullptr);
+      CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr);
       switch (constraint.tag) {
         case Constraint::Tag::INTRA_SUBMAP:
-          CHECK(submap_data_.at(constraint.submap_id)
+          CHECK(data_.submap_data.at(constraint.submap_id)
                     .node_ids.emplace(constraint.node_id)
                     .second);
           break;
@@ -556,7 +573,7 @@ void PoseGraph3D::AddSerializedConstraints(
           UpdateTrajectoryConnectivity(constraint);
           break;
       }
-      constraints_.push_back(constraint);
+      data_.constraints.push_back(constraint);
     }
     LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
   });
@@ -591,14 +608,14 @@ void PoseGraph3D::RunFinalOptimization() {
 void PoseGraph3D::LogResidualHistograms() const {
   common::Histogram rotational_residual;
   common::Histogram translational_residual;
-  for (const Constraint& constraint : constraints_) {
+  for (const Constraint& constraint : data_.constraints) {
     if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
       const cartographer::transform::Rigid3d optimized_node_to_map =
-          trajectory_nodes_.at(constraint.node_id).global_pose;
+          data_.trajectory_nodes.at(constraint.node_id).global_pose;
       const cartographer::transform::Rigid3d node_to_submap_constraint =
           constraint.pose.zbar_ij;
       const cartographer::transform::Rigid3d optimized_submap_to_map =
-          global_submap_poses_.at(constraint.submap_id).global_pose;
+          data_.global_submap_poses_3d.at(constraint.submap_id).global_pose;
       const cartographer::transform::Rigid3d optimized_node_to_submap =
           optimized_submap_to_map.inverse() * optimized_node_to_map;
       const cartographer::transform::Rigid3d residual =
@@ -619,44 +636,45 @@ void PoseGraph3D::RunOptimization() {
     return;
   }
 
-  // No other thread is accessing the optimization_problem_, constraints_,
+  // No other thread is accessing the optimization_problem_, data_.constraints,
-  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
+  // data_.frozen_trajectories and data_.landmark_nodes when executing the
-  // time consuming, so not taking the mutex before Solve to avoid blocking
+  // Solve. Solve is time consuming, so not taking the mutex before Solve to
-  // foreground processing.
+  // avoid blocking foreground processing.
-  optimization_problem_->Solve(constraints_, frozen_trajectories_,
+  optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories,
-                               landmark_nodes_);
+                               data_.landmark_nodes);
   common::MutexLocker locker(&mutex_);
 
   const auto& submap_data = optimization_problem_->submap_data();
   const auto& node_data = optimization_problem_->node_data();
   for (const int trajectory_id : node_data.trajectory_ids()) {
     for (const auto& node : node_data.trajectory(trajectory_id)) {
-      trajectory_nodes_.at(node.id).global_pose = node.data.global_pose;
+      data_.trajectory_nodes.at(node.id).global_pose = node.data.global_pose;
     }
 
     // Extrapolate all point cloud poses that were not included in the
     // 'optimization_problem_' yet.
     const auto local_to_new_global =
         ComputeLocalToGlobalTransform(submap_data, trajectory_id);
-    const auto local_to_old_global =
+    const auto local_to_old_global = ComputeLocalToGlobalTransform(
-        ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
+        data_.global_submap_poses_3d, trajectory_id);
     const transform::Rigid3d old_global_to_new_global =
         local_to_new_global * local_to_old_global.inverse();
 
     const NodeId last_optimized_node_id =
         std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
-    auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
+    auto node_it =
-    for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
+        std::next(data_.trajectory_nodes.find(last_optimized_node_id));
+    for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
          ++node_it) {
-      auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
+      auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
       mutable_trajectory_node.global_pose =
           old_global_to_new_global * mutable_trajectory_node.global_pose;
     }
   }
   for (const auto& landmark : optimization_problem_->landmark_data()) {
-    landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
+    data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
   }
-  global_submap_poses_ = submap_data;
+  data_.global_submap_poses_3d = submap_data;
 
   // Log the histograms for the pose residuals.
   if (options_.log_residual_histograms()) {
@@ -666,14 +684,14 @@ void PoseGraph3D::RunOptimization() {
 
 MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const {
   common::MutexLocker locker(&mutex_);
-  return trajectory_nodes_;
+  return data_.trajectory_nodes;
 }
 
 MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
     const {
   MapById<NodeId, TrajectoryNodePose> node_poses;
   common::MutexLocker locker(&mutex_);
-  for (const auto& node_id_data : trajectory_nodes_) {
+  for (const auto& node_id_data : data_.trajectory_nodes) {
     common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
     if (node_id_data.data.constant_data != nullptr) {
       constant_pose_data = TrajectoryNodePose::ConstantPoseData{
@@ -691,7 +709,7 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
     const {
   std::map<std::string, transform::Rigid3d> landmark_poses;
   common::MutexLocker locker(&mutex_);
-  for (const auto& landmark : landmark_nodes_) {
+  for (const auto& landmark : data_.landmark_nodes) {
     // Landmark without value has not been optimized yet.
     if (!landmark.second.global_landmark_pose.has_value()) continue;
     landmark_poses[landmark.first] =
@@ -704,7 +722,7 @@ void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
                                   const transform::Rigid3d& global_pose) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([=]() REQUIRES(mutex_) {
-    landmark_nodes_[landmark_id].global_landmark_pose = global_pose;
+    data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
   });
 }
 
@@ -727,7 +745,7 @@ PoseGraph3D::GetFixedFramePoseData() const {
 std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
 PoseGraph3D::GetLandmarkNodes() const {
   common::MutexLocker locker(&mutex_);
-  return landmark_nodes_;
+  return data_.landmark_nodes;
 }
 
 std::map<int, PoseGraphInterface::TrajectoryData>
@@ -738,7 +756,7 @@ PoseGraph3D::GetTrajectoryData() const {
 
 std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const {
   common::MutexLocker locker(&mutex_);
-  return constraints_;
+  return data_.constraints;
 }
 
 void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id,
@@ -746,19 +764,20 @@ void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id,
                                            const transform::Rigid3d& pose,
                                            const common::Time time) {
   common::MutexLocker locker(&mutex_);
-  initial_trajectory_poses_[from_trajectory_id] =
+  data_.initial_trajectory_poses[from_trajectory_id] =
       InitialTrajectoryPose{to_trajectory_id, pose, time};
 }
 
 transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
     const int trajectory_id, const common::Time time) const {
-  CHECK_GT(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id), 0);
+  CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
-  const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
+  const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);
-  if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) {
+  if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) {
-    return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose;
+    return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)
+        ->data.global_pose;
   }
-  if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) {
+  if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) {
-    return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id))
+    return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
         ->data.global_pose;
   }
   return transform::Interpolate(
@@ -773,11 +792,12 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
 transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
     const int trajectory_id) const {
   common::MutexLocker locker(&mutex_);
-  return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
+  return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d,
+                                       trajectory_id);
 }
 
 std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() const {
-  return trajectory_connectivity_state_.Components();
+  return data_.trajectory_connectivity_state.Components();
 }
 
 PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
@@ -796,7 +816,7 @@ MapById<SubmapId, PoseGraphInterface::SubmapPose>
 PoseGraph3D::GetAllSubmapPoses() const {
   common::MutexLocker locker(&mutex_);
   MapById<SubmapId, SubmapPose> submap_poses;
-  for (const auto& submap_id_data : submap_data_) {
+  for (const auto& submap_id_data : data_.submap_data) {
     auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
     submap_poses.Insert(
         submap_id_data.id,
@@ -812,8 +832,8 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
   auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
   auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
   if (begin_it == end_it) {
-    const auto it = initial_trajectory_poses_.find(trajectory_id);
+    const auto it = data_.initial_trajectory_poses.find(trajectory_id);
-    if (it != initial_trajectory_poses_.end()) {
+    if (it != data_.initial_trajectory_poses.end()) {
       return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                  it->second.time) *
              it->second.relative_pose;
@@ -824,24 +844,24 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
   const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
   // Accessing 'local_pose' in Submap is okay, since the member is const.
   return global_submap_poses.at(last_optimized_submap_id).global_pose *
-         submap_data_.at(last_optimized_submap_id)
+         data_.submap_data.at(last_optimized_submap_id)
              .submap->local_pose()
              .inverse();
 }
 
 PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock(
     const SubmapId& submap_id) const {
-  const auto it = submap_data_.find(submap_id);
+  const auto it = data_.submap_data.find(submap_id);
-  if (it == submap_data_.end()) {
+  if (it == data_.submap_data.end()) {
     return {};
   }
   auto submap = it->data.submap;
-  if (global_submap_poses_.Contains(submap_id)) {
+  if (data_.global_submap_poses_3d.Contains(submap_id)) {
     // We already have an optimized pose.
-    return {submap, global_submap_poses_.at(submap_id).global_pose};
+    return {submap, data_.global_submap_poses_3d.at(submap_id).global_pose};
   }
   // We have to extrapolate.
-  return {submap, ComputeLocalToGlobalTransform(global_submap_poses_,
+  return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_3d,
                                                 submap_id.trajectory_id) *
                       submap->local_pose()};
 }
@@ -866,28 +886,28 @@ std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
 MapById<SubmapId, PoseGraphInterface::SubmapData>
 PoseGraph3D::TrimmingHandle::GetOptimizedSubmapData() const {
   MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
-  for (const auto& submap_id_data : parent_->submap_data_) {
+  for (const auto& submap_id_data : parent_->data_.submap_data) {
     if (submap_id_data.data.state != SubmapState::kFinished ||
-        !parent_->global_submap_poses_.Contains(submap_id_data.id)) {
+        !parent_->data_.global_submap_poses_3d.Contains(submap_id_data.id)) {
       continue;
     }
     submaps.Insert(
         submap_id_data.id,
-        SubmapData{
+        SubmapData{submap_id_data.data.submap,
-            submap_id_data.data.submap,
+                   parent_->data_.global_submap_poses_3d.at(submap_id_data.id)
-            parent_->global_submap_poses_.at(submap_id_data.id).global_pose});
+                       .global_pose});
   }
   return submaps;
 }
 
 const MapById<NodeId, TrajectoryNode>&
 PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const {
-  return parent_->trajectory_nodes_;
+  return parent_->data_.trajectory_nodes;
 }
 
 const std::vector<PoseGraphInterface::Constraint>&
 PoseGraph3D::TrimmingHandle::GetConstraints() const {
-  return parent_->constraints_;
+  return parent_->data_.constraints;
 }
 
 bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const {
@@ -898,22 +918,23 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
     const SubmapId& submap_id) {
   // TODO(hrapp): We have to make sure that the trajectory has been finished
   // if we want to delete the last submaps.
-  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
+  CHECK(parent_->data_.submap_data.at(submap_id).state ==
+        SubmapState::kFinished);
 
   // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
   // with 'submap_id' is gone.
   std::set<NodeId> nodes_to_retain;
-  for (const Constraint& constraint : parent_->constraints_) {
+  for (const Constraint& constraint : parent_->data_.constraints) {
     if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
         constraint.submap_id != submap_id) {
       nodes_to_retain.insert(constraint.node_id);
     }
   }
-  // Remove all 'constraints_' related to 'submap_id'.
+  // Remove all 'data_.constraints' related to 'submap_id'.
   std::set<NodeId> nodes_to_remove;
   {
     std::vector<Constraint> constraints;
-    for (const Constraint& constraint : parent_->constraints_) {
+    for (const Constraint& constraint : parent_->data_.constraints) {
       if (constraint.submap_id == submap_id) {
         if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
             nodes_to_retain.count(constraint.node_id) == 0) {
@@ -925,29 +946,30 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
         constraints.push_back(constraint);
       }
     }
-    parent_->constraints_ = std::move(constraints);
+    parent_->data_.constraints = std::move(constraints);
   }
-  // Remove all 'constraints_' related to 'nodes_to_remove'.
+  // Remove all 'data_.constraints' related to 'nodes_to_remove'.
   {
     std::vector<Constraint> constraints;
-    for (const Constraint& constraint : parent_->constraints_) {
+    for (const Constraint& constraint : parent_->data_.constraints) {
       if (nodes_to_remove.count(constraint.node_id) == 0) {
         constraints.push_back(constraint);
       }
     }
-    parent_->constraints_ = std::move(constraints);
+    parent_->data_.constraints = std::move(constraints);
   }
 
   // Mark the submap with 'submap_id' as trimmed and remove its data.
-  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
+  CHECK(parent_->data_.submap_data.at(submap_id).state ==
-  parent_->submap_data_.Trim(submap_id);
+        SubmapState::kFinished);
+  parent_->data_.submap_data.Trim(submap_id);
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
   parent_->optimization_problem_->TrimSubmap(submap_id);
 
   // Remove the 'nodes_to_remove' from the pose graph and the optimization
   // problem.
   for (const NodeId& node_id : nodes_to_remove) {
-    parent_->trajectory_nodes_.Trim(node_id);
+    parent_->data_.trajectory_nodes.Trim(node_id);
     parent_->optimization_problem_->TrimTrajectoryNode(node_id);
   }
 }
@@ -955,7 +977,7 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
 MapById<SubmapId, PoseGraphInterface::SubmapData>
 PoseGraph3D::GetSubmapDataUnderLock() const {
   MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
-  for (const auto& submap_id_data : submap_data_) {
+  for (const auto& submap_id_data : data_.submap_data) {
     submaps.Insert(submap_id_data.id,
                    GetSubmapDataUnderLock(submap_id_data.id));
   }

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -37,6 +37,7 @@
 #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
 #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
 #include "cartographer/mapping/pose_graph.h"
+#include "cartographer/mapping/pose_graph_data.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/landmark_data.h"
@@ -150,21 +151,6 @@ class PoseGraph3D : public PoseGraph {
   void WaitForAllComputations() EXCLUDES(mutex_);
 
  private:
-  // The current state of the submap in the background threads. When this
-  // transitions to kFinished, all nodes are tried to match against this submap.
-  // Likewise, all new nodes are matched against submaps which are finished.
-  enum class SubmapState { kActive, kFinished };
-  struct InternalSubmapData {
-    std::shared_ptr<const Submap3D> submap;
-
-    // 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 'finished'.
-    std::set<NodeId> node_ids;
-
-    SubmapState state = SubmapState::kActive;
-  };
-
   MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const REQUIRES(mutex_);
 
   // Handles a new work item.
@@ -235,9 +221,6 @@ class PoseGraph3D : public PoseGraph {
   std::unique_ptr<std::deque<std::function<void()>>> work_queue_
       GUARDED_BY(mutex_);
 
-  // How our various trajectories are related.
-  TrajectoryConnectivityState trajectory_connectivity_state_;
-
   // We globally localize a fraction of the nodes from each trajectory.
   std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
       global_localization_samplers_ GUARDED_BY(mutex_);
@@ -251,36 +234,11 @@ class PoseGraph3D : public PoseGraph {
   // Current optimization problem.
   std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
   constraints::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
-  std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
-
-  // 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_ GUARDED_BY(mutex_);
-
-  // Data that are currently being shown.
-  MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
-  int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
-
-  // Global submap poses currently used for displaying data.
-  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_
-      GUARDED_BY(mutex_);
-
-  // Global landmark poses with all observations.
-  std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
-      landmark_nodes_ GUARDED_BY(mutex_);
 
   // List of all trimmers to consult when optimizations finish.
   std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);
 
-  // Set of all frozen trajectories not being optimized.
+  PoseGraphData data_ GUARDED_BY(mutex_);
-  std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
-
-  // Set of all finished trajectories.
-  std::set<int> finished_trajectories_ GUARDED_BY(mutex_);
-
-  // Set of all initial trajectory poses.
-  std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
-      GUARDED_BY(mutex_);
 
   // Allows querying and manipulating the pose graph by the 'trimmers_'. The
   // 'mutex_' of the pose graph is held while this class is used.