Fix PureLocalizationTrimmer (#1002)

Remove the wrong assumption that submap index is sequential from 0.

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

@@ -781,6 +781,16 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
   return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
 }
 
+std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
+    int trajectory_id) const {
+  std::vector<SubmapId> submap_ids;
+  const auto& submap_data = parent_->optimization_problem_->submap_data();
+  for (const auto& it : submap_data.trajectory(trajectory_id)) {
+    submap_ids.push_back(it.id);
+  }
+  return submap_ids;
+}
+
 bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
   return parent_->IsTrajectoryFinished(trajectory_id);
 }

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

@@ -276,6 +276,7 @@ class PoseGraph2D : public PoseGraph {
     ~TrimmingHandle() override {}
 
     int num_submaps(int trajectory_id) const override;
+    std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
     void MarkSubmapAsTrimmed(const SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
     bool IsFinished(int trajectory_id) const override;

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

@@ -812,6 +812,16 @@ int PoseGraph3D::TrimmingHandle::num_submaps(const int trajectory_id) const {
   return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
 }
 
+std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
+    int trajectory_id) const {
+  std::vector<SubmapId> submap_ids;
+  const auto& submap_data = parent_->optimization_problem_->submap_data();
+  for (const auto& it : submap_data.trajectory(trajectory_id)) {
+    submap_ids.push_back(it.id);
+  }
+  return submap_ids;
+}
+
 bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const {
   return parent_->IsTrajectoryFinished(trajectory_id);
 }

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

@@ -281,6 +281,7 @@ class PoseGraph3D : public PoseGraph {
     ~TrimmingHandle() override {}
 
     int num_submaps(int trajectory_id) const override;
+    std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
     void MarkSubmapAsTrimmed(const SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
     bool IsFinished(int trajectory_id) const override;

cartographer/mapping/pose_graph_trimmer.cc

@@ -32,11 +32,9 @@ void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
     num_submaps_to_keep_ = 0;
   }
 
-  while (pose_graph->num_submaps(trajectory_id_) > num_submaps_to_keep_) {
-    const int submap_index_to_trim_next = num_submaps_trimmed_;
-    pose_graph->MarkSubmapAsTrimmed(
-        SubmapId{trajectory_id_, submap_index_to_trim_next});
-    ++num_submaps_trimmed_;
+  auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
+  for (int i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) {
+    pose_graph->MarkSubmapAsTrimmed(submap_ids.at(i));
   }
 
   if (num_submaps_to_keep_ == 0) {

cartographer/mapping/pose_graph_trimmer.h

@@ -28,10 +28,10 @@ class Trimmable {
  public:
   virtual ~Trimmable() {}
 
-  // TODO(whess): This is all the functionality necessary for pure localization.
-  // To be expanded as needed for lifelong mapping.
   virtual int num_submaps(int trajectory_id) const = 0;
 
+  virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
+
   // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They
   // will no longer take part in scan matching, loop closure, visualization.
   // Submaps and nodes are only marked, the numbering remains unchanged.
@@ -66,7 +66,6 @@ class PureLocalizationTrimmer : public PoseGraphTrimmer {
  private:
   const int trajectory_id_;
   int num_submaps_to_keep_;
-  int num_submaps_trimmed_ = 0;
   bool finished_ = false;
 };
 

cartographer/mapping/pose_graph_trimmer_test.cc

@@ -27,10 +27,19 @@ namespace {
 
 class FakePoseGraph : public Trimmable {
  public:
+  FakePoseGraph(int trajectory_id, int num_submaps) {
+    for (int index = 0; index < num_submaps; ++index) {
+      submaps_.push_back(SubmapId{trajectory_id, index});
+    }
+  }
   ~FakePoseGraph() override {}
 
   int num_submaps(const int trajectory_id) const override {
-    return 17 - trimmed_submaps_.size();
+    return submaps_.size() - trimmed_submaps_.size();
+  }
+
+  std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override {
+    return submaps_;
   }
 
   void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
@@ -42,13 +51,14 @@ class FakePoseGraph : public Trimmable {
   std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
 
  private:
+  std::vector<SubmapId> submaps_;
   std::vector<SubmapId> trimmed_submaps_;
 };
 
 TEST(PureLocalizationTrimmerTest, MarksSubmapsAsExpected) {
   const int kTrajectoryId = 42;
   PureLocalizationTrimmer trimmer(kTrajectoryId, 15);
-  FakePoseGraph fake_pose_graph;
+  FakePoseGraph fake_pose_graph(kTrajectoryId, 17);
   trimmer.Trim(&fake_pose_graph);
 
   const auto trimmed_submaps = fake_pose_graph.trimmed_submaps();