cartographer-project · ojura · Sep 6, 2018 · Sep 6, 2018 · Sep 6, 2018 · Sep 6, 2018
diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc
@@ -52,6 +52,11 @@ PoseGraphStub::GetAllSubmapData() const {
   LOG(FATAL) << "Not implemented";
 }
 
+mapping::PoseGraphInterface::SubmapData PoseGraphStub::GetSubmapData(
+    const mapping::SubmapId& submap_id) const {
+  LOG(FATAL) << "Not implemented";
+}
+
 mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
 PoseGraphStub::GetAllSubmapPoses() const {
   google::protobuf::Empty request;

diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h
@@ -34,6 +34,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   void RunFinalOptimization() override;
   mapping::MapById<mapping::SubmapId, SubmapData> GetAllSubmapData()
       const override;
+  SubmapData GetSubmapData(const mapping::SubmapId& submap_id) const override;
   mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
       const override;
   transform::Rigid3d GetLocalToGlobalTransform(

diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -123,7 +123,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,
@@ -148,18 +148,29 @@ NodeId PoseGraph2D::AppendNode(
     LOG(INFO) << "Inserted submap " << submap_id << ".";
     kActiveSubmapsMetric->Increment();
   }
-  return node_id;
+  std::vector<SubmapId> submap_ids;
+  auto submap_data_iter =
+      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id),
+                insertion_submaps.size());
+  for (int i = 0; i < static_cast<int>(insertion_submaps.size()); ++i) {
+    CHECK_EQ(submap_data_iter->data.submap, insertion_submaps.at(i));
+    submap_ids.push_back(submap_data_iter->id);
+    ++submap_data_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 =
@@ -168,7 +179,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)
@@ -173,7 +173,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
@@ -111,7 +111,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,
@@ -136,18 +136,29 @@ NodeId PoseGraph3D::AppendNode(
     LOG(INFO) << "Inserted submap " << submap_id << ".";
     kActiveSubmapsMetric->Increment();
   }
-  return node_id;
+  std::vector<SubmapId> submap_ids;
+  auto submap_data_iter =
+      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id),
+                insertion_submaps.size());
+  for (int i = 0; i < static_cast<int>(insertion_submaps.size()); ++i) {
+    CHECK_EQ(submap_data_iter->data.submap, insertion_submaps.at(i));
+    submap_ids.push_back(submap_data_iter->id);
+    ++submap_data_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 =
@@ -156,7 +167,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)
@@ -176,7 +176,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,12 +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,
+          node_submap_ids.second,
           std::vector<std::shared_ptr<const Submap>>(
               matching_result->insertion_result->insertion_submaps.begin(),
               matching_result->insertion_result->insertion_submaps.end())});

diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h
@@ -34,6 +34,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
   MOCK_METHOD0(RunFinalOptimization, void());
   MOCK_CONST_METHOD0(GetAllSubmapData,
                      mapping::MapById<mapping::SubmapId, SubmapData>());
+  MOCK_CONST_METHOD1(GetSubmapData, SubmapData(const mapping::SubmapId&));
   MOCK_CONST_METHOD0(GetAllSubmapPoses,
                      mapping::MapById<mapping::SubmapId, SubmapPose>());
   MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int));

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
@@ -96,6 +96,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,6 +50,7 @@ class TrajectoryBuilderInterface {
   struct InsertionResult {
     NodeId node_id;
     std::shared_ptr<const TrajectoryNode::Data> constant_data;
+    std::vector<SubmapId> insertion_submap_ids;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };