From 88805a301d11e6e711bfcaa8817182a1b708f136 Mon Sep 17 00:00:00 2001 From: Jihoon Lee Date: Tue, 28 Nov 2017 10:21:55 +0100 Subject: [PATCH] Trim all submaps when pure localization trajectory is finished. (#563) Fixes #560 based on #562 --- cartographer/mapping/pose_graph.h | 3 ++ cartographer/mapping/pose_graph_trimmer.cc | 10 +++++++ cartographer/mapping/pose_graph_trimmer.h | 10 ++++++- .../mapping/pose_graph_trimmer_test.cc | 4 ++- cartographer/mapping_2d/pose_graph.cc | 28 +++++++++++++++++-- cartographer/mapping_2d/pose_graph.h | 5 ++++ cartographer/mapping_3d/pose_graph.cc | 28 +++++++++++++++++-- cartographer/mapping_3d/pose_graph.h | 5 ++++ 8 files changed, 87 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 33b3108..8d35b7a 100644 --- a/cartographer/mapping/pose_graph.h +++ b/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; diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index 9103e5b..6c1a649 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/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 diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h index 5e87a4d..a39b429 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/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 diff --git a/cartographer/mapping/pose_graph_trimmer_test.cc b/cartographer/mapping/pose_graph_trimmer_test.cc index 8c63bfb..5b924a7 100644 --- a/cartographer/mapping/pose_graph_trimmer_test.cc +++ b/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 trimmed_submaps() { return trimmed_submaps_; } private: diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 60ac1ff..02dc11b 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/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& 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 diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index 6d500e1..c8af7d0 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/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 frozen_trajectories_ GUARDED_BY(mutex_); + // Set of all finished trajectories. + std::set finished_trajectories_ GUARDED_BY(mutex_); + // Set of all initial trajectory poses. std::map 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_; diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index 0a070b3..fdee1bf 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/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& 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 diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index e9c1efd..c68b725 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/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 frozen_trajectories_ GUARDED_BY(mutex_); + // Set of all finished trajectories. + std::set finished_trajectories_ GUARDED_BY(mutex_); + // Set of all initial trajectory poses. std::map 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_;