Trim all submaps when pure localization trajectory is finished. (#563)

Fixes #560 
based on #562

cartographer/mapping/pose_graph.h

@@ -100,6 +100,9 @@ class PoseGraph {
   // Finishes the given trajectory.
   virtual void FinishTrajectory(int trajectory_id) = 0;
 
+  // Checks if the given trajectory is finished.
+  virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
+
   // Freezes a trajectory. Poses in this trajectory will not be optimized.
   virtual void FreezeTrajectory(int trajectory_id) = 0;
 

cartographer/mapping/pose_graph_trimmer.cc

@@ -28,13 +28,23 @@ PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
 }
 
 void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
+  if (pose_graph->IsFinished(trajectory_id_)) {
+    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_;
   }
+
+  if (num_submaps_to_keep_ == 0) {
+    finished_ = true;
+  }
 }
 
+bool PureLocalizationTrimmer::IsFinished() { return finished_; }
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/pose_graph_trimmer.h

@@ -36,6 +36,9 @@ class Trimmable {
   // will no longer take part in scan matching, loop closure, visualization.
   // Submaps and nodes are only marked, the numbering remains unchanged.
   virtual void MarkSubmapAsTrimmed(const SubmapId& submap_id) = 0;
+
+  // Checks if the given trajectory is finished or not.
+  virtual bool IsFinished(int trajectory_id) const = 0;
 };
 
 // An interface to implement algorithms that choose how to trim the pose graph.
@@ -45,6 +48,9 @@ class PoseGraphTrimmer {
 
   // Called once after each pose graph optimization.
   virtual void Trim(Trimmable* pose_graph) = 0;
+
+  // Checks if this trimmer is in a terminatable state.
+  virtual bool IsFinished() = 0;
 };
 
 // Keeps the last 'num_submaps_to_keep' of the trajectory with 'trajectory_id'
@@ -55,11 +61,13 @@ class PureLocalizationTrimmer : public PoseGraphTrimmer {
   ~PureLocalizationTrimmer() override {}
 
   void Trim(Trimmable* pose_graph) override;
+  bool IsFinished() override;
 
  private:
   const int trajectory_id_;
-  const int num_submaps_to_keep_;
+  int num_submaps_to_keep_;
   int num_submaps_trimmed_ = 0;
+  bool finished_ = false;
 };
 
 }  // namespace mapping

cartographer/mapping/pose_graph_trimmer_test.cc

@@ -29,7 +29,7 @@ class FakePoseGraph : public Trimmable {
  public:
   ~FakePoseGraph() override {}
 
-  int num_submaps(int trajectory_id) const override {
+  int num_submaps(const int trajectory_id) const override {
     return 17 - trimmed_submaps_.size();
   }
 
@@ -37,6 +37,8 @@ class FakePoseGraph : public Trimmable {
     trimmed_submaps_.push_back(submap_id);
   }
 
+  bool IsFinished(const int trajectory_id) const override { return false; }
+
   std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
 
  private:

cartographer/mapping_2d/pose_graph.cc

@@ -320,6 +320,13 @@ void PoseGraph::HandleWorkQueue() {
         for (auto& trimmer : trimmers_) {
           trimmer->Trim(&trimming_handle);
         }
+        trimmers_.erase(
+            std::remove_if(
+                trimmers_.begin(), trimmers_.end(),
+                [](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) {
+                  return trimmer->IsFinished();
+                }),
+            trimmers_.end());
 
         num_nodes_since_last_loop_closure_ = 0;
         run_loop_closure_ = false;
@@ -369,8 +376,21 @@ void PoseGraph::WaitForAllComputations() {
 }
 
 void PoseGraph::FinishTrajectory(const int trajectory_id) {
-  // TODO(jihoonl): Add a logic to notify trimmers to finish the given
-  // trajectory.
+  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+    CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
+    finished_trajectories_.insert(trajectory_id);
+
+    auto submap_data = optimization_problem_.submap_data();
+    for (auto submap_id_data : submap_data) {
+      submap_data_.at(submap_id_data.id).state = SubmapState::kFinished;
+    }
+    // TODO(jihoonl): Refactor HandleWorkQueue() logic from
+    // ComputeConstraintsForNode and call from here
+  });
+}
+
+bool PoseGraph::IsTrajectoryFinished(const int trajectory_id) {
+  return finished_trajectories_.count(trajectory_id) > 0;
 }
 
 void PoseGraph::FreezeTrajectory(const int trajectory_id) {
@@ -684,6 +704,10 @@ int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
   return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
 }
 
+bool PoseGraph::TrimmingHandle::IsFinished(const int trajectory_id) const {
+  return parent_->IsTrajectoryFinished(trajectory_id);
+}
+
 void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     const mapping::SubmapId& submap_id) {
   // TODO(hrapp): We have to make sure that the trajectory has been finished

cartographer/mapping_2d/pose_graph.h

@@ -87,6 +87,7 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_);
 
   void FinishTrajectory(int trajectory_id) override;
+  bool IsTrajectoryFinished(int trajectory_id) override;
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
                           const mapping::proto::Submap& submap) override;
@@ -240,6 +241,9 @@ class PoseGraph : public mapping::PoseGraph {
   // Set of all frozen trajectories not being optimized.
   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_);
@@ -254,6 +258,7 @@ class PoseGraph : public mapping::PoseGraph {
     int num_submaps(int trajectory_id) const override;
     void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
+    bool IsFinished(int trajectory_id) const override;
 
    private:
     PoseGraph* const parent_;

cartographer/mapping_3d/pose_graph.cc

@@ -337,6 +337,13 @@ void PoseGraph::HandleWorkQueue() {
         for (auto& trimmer : trimmers_) {
           trimmer->Trim(&trimming_handle);
         }
+        trimmers_.erase(
+            std::remove_if(
+                trimmers_.begin(), trimmers_.end(),
+                [](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) {
+                  return trimmer->IsFinished();
+                }),
+            trimmers_.end());
 
         num_nodes_since_last_loop_closure_ = 0;
         run_loop_closure_ = false;
@@ -386,8 +393,21 @@ void PoseGraph::WaitForAllComputations() {
 }
 
 void PoseGraph::FinishTrajectory(const int trajectory_id) {
-  // TODO(jihoonl): Add a logic to notify trimmers to finish the given
-  // trajectory.
+  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+    CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
+    finished_trajectories_.insert(trajectory_id);
+
+    auto submap_data = optimization_problem_.submap_data();
+    for (auto submap_id_data : submap_data) {
+      submap_data_.at(submap_id_data.id).state = SubmapState::kFinished;
+    }
+    // TODO(jihoonl): Refactor HandleWorkQueue() logic from
+    // ComputeConstraintsForNode and call from here
+  });
+}
+
+bool PoseGraph::IsTrajectoryFinished(const int trajectory_id) {
+  return finished_trajectories_.count(trajectory_id) > 0;
 }
 
 void PoseGraph::FreezeTrajectory(const int trajectory_id) {
@@ -700,6 +720,10 @@ int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
   return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
 }
 
+bool PoseGraph::TrimmingHandle::IsFinished(const int trajectory_id) const {
+  return parent_->IsTrajectoryFinished(trajectory_id);
+}
+
 void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     const mapping::SubmapId& submap_id) {
   // TODO(hrapp): We have to make sure that the trajectory has been finished

cartographer/mapping_3d/pose_graph.h

@@ -87,6 +87,7 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_);
 
   void FinishTrajectory(int trajectory_id) override;
+  bool IsTrajectoryFinished(int trajectory_id) override;
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
                           const mapping::proto::Submap& submap) override;
@@ -244,6 +245,9 @@ class PoseGraph : public mapping::PoseGraph {
   // Set of all frozen trajectories not being optimized.
   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_);
@@ -258,6 +262,7 @@ class PoseGraph : public mapping::PoseGraph {
     int num_submaps(int trajectory_id) const override;
     void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
+    bool IsFinished(int trajectory_id) const override;
 
    private:
     PoseGraph* const parent_;