From de22b9c311c21c4290b9372bc9af3e2fbb6a1685 Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 12 Jun 2018 16:01:14 +0200 Subject: [PATCH] Rename to TrimSubmap (#1192) Trimmable::MarkSubmapAsTrimmed was incorrectly named and commented. It really trims the submap. --- .../mapping/internal/2d/overlapping_submaps_trimmer_2d.cc | 2 +- cartographer/mapping/internal/2d/pose_graph_2d.cc | 3 +-- cartographer/mapping/internal/2d/pose_graph_2d.h | 2 +- cartographer/mapping/internal/3d/pose_graph_3d.cc | 3 +-- cartographer/mapping/internal/3d/pose_graph_3d.h | 2 +- cartographer/mapping/internal/3d/pose_graph_3d_test.cc | 2 +- cartographer/mapping/internal/testing/fake_trimmable.h | 2 +- cartographer/mapping/pose_graph_trimmer.cc | 2 +- cartographer/mapping/pose_graph_trimmer.h | 6 +++--- 9 files changed, 11 insertions(+), 13 deletions(-) diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 5314aa6..863e428 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -203,7 +203,7 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { min_covered_cells_count_); current_submap_count_ = submap_data.size() - submap_ids_to_remove.size(); for (const SubmapId& id : submap_ids_to_remove) { - pose_graph->MarkSubmapAsTrimmed(id); + pose_graph->TrimSubmap(id); } } diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 0143753..2b5d74c 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -887,8 +887,7 @@ bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const { return parent_->IsTrajectoryFinished(trajectory_id); } -void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed( - const SubmapId& submap_id) { +void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. CHECK(parent_->data_.submap_data.at(submap_id).state == diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index c211cf6..3c5b07f 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -252,7 +252,7 @@ class PoseGraph2D : public PoseGraph { REQUIRES(parent_->mutex_); const std::vector& GetConstraints() const override REQUIRES(parent_->mutex_); - void MarkSubmapAsTrimmed(const SubmapId& submap_id) + void TrimSubmap(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index dfaa331..661fccd 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -914,8 +914,7 @@ bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const { return parent_->IsTrajectoryFinished(trajectory_id); } -void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed( - const SubmapId& submap_id) { +void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. CHECK(parent_->data_.submap_data.at(submap_id).state == diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 9e0bfef..6376986 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -255,7 +255,7 @@ class PoseGraph3D : public PoseGraph { REQUIRES(parent_->mutex_); const std::vector& GetConstraints() const override REQUIRES(parent_->mutex_); - void MarkSubmapAsTrimmed(const SubmapId& submap_id) + void TrimSubmap(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index f961d65..76e7f86 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -203,7 +203,7 @@ class EvenSubmapTrimmer : public PoseGraphTrimmer { auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); for (const auto submap_id : submap_ids) { if (submap_id.submap_index % 2 == 0) { - pose_graph->MarkSubmapAsTrimmed(submap_id); + pose_graph->TrimSubmap(submap_id); } } } diff --git a/cartographer/mapping/internal/testing/fake_trimmable.h b/cartographer/mapping/internal/testing/fake_trimmable.h index 29113d3..8821cff 100644 --- a/cartographer/mapping/internal/testing/fake_trimmable.h +++ b/cartographer/mapping/internal/testing/fake_trimmable.h @@ -90,7 +90,7 @@ class FakeTrimmable : public Trimmable { return constraints_; } - void MarkSubmapAsTrimmed(const SubmapId& submap_id) override { + void TrimSubmap(const SubmapId& submap_id) override { trimmed_submaps_.push_back(submap_id); } diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index 530ad62..d6ba14e 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/cartographer/mapping/pose_graph_trimmer.cc @@ -34,7 +34,7 @@ void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) { - pose_graph->MarkSubmapAsTrimmed(submap_ids.at(i)); + pose_graph->TrimSubmap(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 3ac1879..6046a99 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/cartographer/mapping/pose_graph_trimmer.h @@ -39,10 +39,10 @@ class Trimmable { virtual const std::vector& GetConstraints() const = 0; - // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They + // Trim 'submap_id' and corresponding intra-submap nodes. They // 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; + // The numbering remains unchanged. + virtual void TrimSubmap(const SubmapId& submap_id) = 0; // Checks if the given trajectory is finished or not. virtual bool IsFinished(int trajectory_id) const = 0;