return submap ids in InsertionResult instead of pointers

ojura · ojura · commit 34a816abdb41 · 2018-09-06T07:18:16.000+02:00
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -117,7 +117,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
   return {front_submap_id, last_submap_id};
 }
 
-NodeId PoseGraph2D::AppendNode(
+std::pair<NodeId, std::vector<SubmapId>> PoseGraph2D::AppendNode(
     std::shared_ptr<const TrajectoryNode::Data> constant_data,
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
@@ -141,18 +141,28 @@ NodeId PoseGraph2D::AppendNode(
     data_.submap_data.at(submap_id).submap = insertion_submaps.back();
     LOG(INFO) << "Inserted submap " << submap_id << ".";
   }
-  return node_id;
+  std::vector<SubmapId> submap_ids;
+  auto submap_id_iter =
+      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id),
+                insertion_submaps.size());
+  for (int i = 0; i < static_cast<int>(insertion_submaps.size()); ++i) {
+    submap_ids.push_back(submap_id_iter->id);
+    ++submap_id_iter;
+  }
+  return {node_id, submap_ids};
 }
 
-NodeId PoseGraph2D::AddNode(
+std::pair<NodeId, std::vector<SubmapId>> PoseGraph2D::AddNode(
     std::shared_ptr<const TrajectoryNode::Data> constant_data,
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
 
-  const NodeId node_id = AppendNode(constant_data, trajectory_id,
-                                    insertion_submaps, optimized_pose);
+  const auto node_submap_ids = AppendNode(constant_data, trajectory_id,
+                                          insertion_submaps, optimized_pose);
+  const NodeId& node_id = node_submap_ids.first;
+
   // We have to check this here, because it might have changed by the time we
   // execute the lambda.
   const bool newly_finished_submap =
@@ -161,7 +171,7 @@ NodeId PoseGraph2D::AddNode(
     return ComputeConstraintsForNode(node_id, insertion_submaps,
                                      newly_finished_submap);
   });
-  return node_id;
+  return node_submap_ids;
 }
 
 void PoseGraph2D::AddWorkItem(
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h
@@ -77,7 +77,7 @@ class PoseGraph2D : public PoseGraph {
   // node data was inserted into the 'insertion_submaps'. If
   // 'insertion_submaps.front().finished()' is 'true', data was inserted into
   // this submap for the last time.
-  NodeId AddNode(
+  std::pair<NodeId, std::vector<SubmapId>> AddNode(
       std::shared_ptr<const TrajectoryNode::Data> constant_data,
       int trajectory_id,
       const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
@@ -171,7 +171,7 @@ class PoseGraph2D : public PoseGraph {
 
   // Appends the new node and submap (if needed) to the internal data
   // structures.
-  NodeId AppendNode(
+  std::pair<NodeId, std::vector<SubmapId>> AppendNode(
       std::shared_ptr<const TrajectoryNode::Data> constant_data,
       int trajectory_id,
       const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -105,7 +105,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
   return {front_submap_id, last_submap_id};
 }
 
-NodeId PoseGraph3D::AppendNode(
+std::pair<NodeId, std::vector<SubmapId>> PoseGraph3D::AppendNode(
     std::shared_ptr<const TrajectoryNode::Data> constant_data,
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps,
@@ -129,18 +129,28 @@ NodeId PoseGraph3D::AppendNode(
     data_.submap_data.at(submap_id).submap = insertion_submaps.back();
     LOG(INFO) << "Inserted submap " << submap_id << ".";
   }
-  return node_id;
+  std::vector<SubmapId> submap_ids;
+  auto submap_id_iter =
+      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id),
+                insertion_submaps.size());
+  for (int i = 0; i < static_cast<int>(insertion_submaps.size()); ++i) {
+    submap_ids.push_back(submap_id_iter->id);
+    ++submap_id_iter;
+  }
+  return {node_id, submap_ids};
 }
 
-NodeId PoseGraph3D::AddNode(
+std::pair<NodeId, std::vector<SubmapId>> PoseGraph3D::AddNode(
     std::shared_ptr<const TrajectoryNode::Data> constant_data,
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
 
-  const NodeId node_id = AppendNode(constant_data, trajectory_id,
-                                    insertion_submaps, optimized_pose);
+  const auto node_submap_ids = AppendNode(constant_data, trajectory_id,
+                                          insertion_submaps, optimized_pose);
+  const NodeId& node_id = node_submap_ids.first;
+
   // We have to check this here, because it might have changed by the time we
   // execute the lambda.
   const bool newly_finished_submap =
@@ -149,7 +159,7 @@ NodeId PoseGraph3D::AddNode(
     return ComputeConstraintsForNode(node_id, insertion_submaps,
                                      newly_finished_submap);
   });
-  return node_id;
+  return node_submap_ids;
 }
 
 void PoseGraph3D::AddWorkItem(
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h
@@ -75,7 +75,7 @@ class PoseGraph3D : public PoseGraph {
   // node data was inserted into the 'insertion_submaps'. If
   // 'insertion_submaps.front().finished()' is 'true', data was inserted into
   // this submap for the last time.
-  NodeId AddNode(
+  std::pair<NodeId, std::vector<SubmapId>> AddNode(
       std::shared_ptr<const TrajectoryNode::Data> constant_data,
       int trajectory_id,
       const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
@@ -174,7 +174,7 @@ class PoseGraph3D : public PoseGraph {
       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
 
   // Appends the new node and submap (if needed) to the internal data stuctures.
-  NodeId AppendNode(
+  std::pair<NodeId, std::vector<SubmapId>> AppendNode(
       std::shared_ptr<const TrajectoryNode::Data> constant_data,
       int trajectory_id,
       const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps,
diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc
@@ -65,15 +65,14 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
     std::unique_ptr<InsertionResult> insertion_result;
     if (matching_result->insertion_result != nullptr) {
       kLocalSlamInsertionResults->Increment();
-      auto node_id = pose_graph_->AddNode(
+      const auto node_submap_ids = pose_graph_->AddNode(
           matching_result->insertion_result->constant_data, trajectory_id_,
           matching_result->insertion_result->insertion_submaps);
+      const auto& node_id = node_submap_ids.first;
       CHECK_EQ(node_id.trajectory_id, trajectory_id_);
       insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
           node_id, matching_result->insertion_result->constant_data,
-          std::vector<std::shared_ptr<const Submap>>(
-              matching_result->insertion_result->insertion_submaps.begin(),
-              matching_result->insertion_result->insertion_submaps.end())});
+          node_submap_ids.second});
     }
     if (local_slam_result_callback_) {
       local_slam_result_callback_(
diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h
@@ -112,11 +112,6 @@ class PoseGraph : public PoseGraphInterface {
   // Gets the current trajectory clusters.
   virtual std::vector<std::vector<int>> GetConnectedTrajectories() const = 0;
 
-  // Returns the current optimized transform and submap itself for the given
-  // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does
-  // not exist (anymore).
-  virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0;
-
   proto::PoseGraph ToProto(bool include_unfinished_submaps) const override;
 
   // Returns the IMU data.
diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
@@ -95,6 +95,11 @@ class PoseGraphInterface {
   // Waits for all computations to finish and computes optimized poses.
   virtual void RunFinalOptimization() = 0;
 
+  // Returns the current optimized transform and submap itself for the given
+  // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does
+  // not exist (anymore).
+  virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0;
+
   // Returns data for all submaps.
   virtual MapById<SubmapId, SubmapData> GetAllSubmapData() const = 0;
 
diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h
@@ -50,7 +50,7 @@ class TrajectoryBuilderInterface {
   struct InsertionResult {
     NodeId node_id;
     std::shared_ptr<const TrajectoryNode::Data> constant_data;
-    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
+    std::vector<SubmapId> insertion_submap_ids;
   };
 
   // A callback which is called after local SLAM processes an accumulated
