From ab7402ffbbd2b3b19c4042a67028ad07fa7f198f Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 19 Apr 2018 17:09:16 +0200 Subject: [PATCH] Use references for constraints & nodes in TrimmingHandle. (#1091) --- .../internal/2d/overlapping_submaps_trimmer_2d.cc | 10 ++++------ cartographer/mapping/internal/2d/pose_graph_2d.cc | 4 ++-- cartographer/mapping/internal/2d/pose_graph_2d.h | 6 +++--- cartographer/mapping/internal/3d/pose_graph_3d.cc | 4 ++-- cartographer/mapping/internal/3d/pose_graph_3d.h | 6 +++--- cartographer/mapping/internal/testing/fake_trimmable.h | 5 +++-- cartographer/mapping/pose_graph_trimmer.h | 4 ++-- 7 files changed, 19 insertions(+), 20 deletions(-) diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 39b9787..bf9515f 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -181,14 +181,12 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { const auto submap_data = pose_graph->GetAllSubmapData(); if (submap_data.size() == 0) return; - const auto constraints = pose_graph->GetConstraints(); - const auto trajectory_nodes = pose_graph->GetTrajectoryNodes(); - SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data)); - + const std::map submap_freshness = + ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(), + pose_graph->GetConstraints()); const std::set all_submap_ids = AddSubmapsToSubmapCoverageGrid2D( - ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints), - submap_data, &coverage_grid); + submap_freshness, submap_data, &coverage_grid); const std::vector submap_ids_to_remove = FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_, min_covered_cells_count_); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index c1dfeeb..a2c6870 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -807,12 +807,12 @@ std::vector PoseGraph2D::TrimmingHandle::GetSubmapIds( return submap_ids; } -MapById +const MapById& PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const { return parent_->trajectory_nodes_; } -std::vector +const std::vector& PoseGraph2D::TrimmingHandle::GetConstraints() const { return parent_->constraints_; } diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 49ad011..4c2c76e 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -284,10 +284,10 @@ class PoseGraph2D : public PoseGraph { std::vector GetSubmapIds(int trajectory_id) const override; MapById GetAllSubmapData() const override REQUIRES(parent_->mutex_); - MapById GetTrajectoryNodes() const override - REQUIRES(parent_->mutex_); - std::vector GetConstraints() const override + const MapById& GetTrajectoryNodes() const override REQUIRES(parent_->mutex_); + const std::vector& GetConstraints() + const override REQUIRES(parent_->mutex_); void MarkSubmapAsTrimmed(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 c572135..99b4ba0 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -836,12 +836,12 @@ PoseGraph3D::TrimmingHandle::GetAllSubmapData() const { return parent_->GetSubmapDataUnderLock(); } -MapById +const MapById& PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const { return parent_->trajectory_nodes_; } -std::vector +const std::vector& PoseGraph3D::TrimmingHandle::GetConstraints() const { return parent_->constraints_; } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index debc8fa..96f02a0 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -289,10 +289,10 @@ class PoseGraph3D : public PoseGraph { std::vector GetSubmapIds(int trajectory_id) const override; MapById GetAllSubmapData() const override REQUIRES(parent_->mutex_); - MapById GetTrajectoryNodes() const override - REQUIRES(parent_->mutex_); - std::vector GetConstraints() const override + const MapById& GetTrajectoryNodes() const override REQUIRES(parent_->mutex_); + const std::vector& GetConstraints() + const override REQUIRES(parent_->mutex_); void MarkSubmapAsTrimmed(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); diff --git a/cartographer/mapping/internal/testing/fake_trimmable.h b/cartographer/mapping/internal/testing/fake_trimmable.h index 707e1b2..cb5f720 100644 --- a/cartographer/mapping/internal/testing/fake_trimmable.h +++ b/cartographer/mapping/internal/testing/fake_trimmable.h @@ -72,7 +72,7 @@ class FakeTrimmable : public Trimmable { return &trajectory_nodes_; } - MapById GetTrajectoryNodes() const override { + const MapById& GetTrajectoryNodes() const override { return trajectory_nodes_; } @@ -85,7 +85,8 @@ class FakeTrimmable : public Trimmable { return &constraints_; } - std::vector GetConstraints() const override { + const std::vector& GetConstraints() + const override { return constraints_; } diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h index 08c7ff9..748026d 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/cartographer/mapping/pose_graph_trimmer.h @@ -34,8 +34,8 @@ class Trimmable { virtual std::vector GetSubmapIds(int trajectory_id) const = 0; virtual MapById GetAllSubmapData() const = 0; - virtual MapById GetTrajectoryNodes() const = 0; - virtual std::vector GetConstraints() + virtual const MapById& GetTrajectoryNodes() const = 0; + virtual const std::vector& GetConstraints() const = 0; // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They