From b23ec8ce4ea72ff1353add2e67f2cf96c51e6c61 Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 19 Mar 2018 10:35:35 +0100 Subject: [PATCH] Fix PureLocalizationTrimmer (#1002) Remove the wrong assumption that submap index is sequential from 0. --- cartographer/mapping/internal/2d/pose_graph_2d.cc | 10 ++++++++++ cartographer/mapping/internal/2d/pose_graph_2d.h | 1 + cartographer/mapping/internal/3d/pose_graph_3d.cc | 10 ++++++++++ cartographer/mapping/internal/3d/pose_graph_3d.h | 1 + cartographer/mapping/pose_graph_trimmer.cc | 8 +++----- cartographer/mapping/pose_graph_trimmer.h | 5 ++--- cartographer/mapping/pose_graph_trimmer_test.cc | 14 ++++++++++++-- 7 files changed, 39 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 994f89c..868ee45 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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 PoseGraph2D::TrimmingHandle::GetSubmapIds( + int trajectory_id) const { + std::vector 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); } diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index c1e3138..2ea8b00 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/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 GetSubmapIds(int trajectory_id) const override; void MarkSubmapAsTrimmed(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override; diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index d1db849..7d87bea 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/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 PoseGraph3D::TrimmingHandle::GetSubmapIds( + int trajectory_id) const { + std::vector 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); } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 5eaea65..0f21c6f 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/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 GetSubmapIds(int trajectory_id) const override; void MarkSubmapAsTrimmed(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override; diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index 6c1a649..a7c7384 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/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) { diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h index a39b429..5bb5936 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/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 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; }; diff --git a/cartographer/mapping/pose_graph_trimmer_test.cc b/cartographer/mapping/pose_graph_trimmer_test.cc index 5b924a7..bcd7ac0 100644 --- a/cartographer/mapping/pose_graph_trimmer_test.cc +++ b/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 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 trimmed_submaps() { return trimmed_submaps_; } private: + std::vector submaps_; std::vector 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();