diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index c832a4b..815739b 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -798,6 +798,16 @@ std::vector PoseGraph2D::TrimmingHandle::GetSubmapIds( return submap_ids; } +MapById +PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const { + return parent_->trajectory_nodes_; +} + +std::vector +PoseGraph2D::TrimmingHandle::GetConstraints() const { + return parent_->constraints_; +} + 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 fa832e1..73d76d7 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -283,6 +283,8 @@ class PoseGraph2D : public PoseGraph { std::vector GetSubmapIds(int trajectory_id) const override; MapById GetAllSubmapData() const override; + MapById GetTrajectoryNodes() const override; + std::vector GetConstraints() 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 9be67a5..638a734 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -828,6 +828,16 @@ PoseGraph3D::TrimmingHandle::GetAllSubmapData() const { return parent_->GetSubmapDataUnderLock(); } +MapById +PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const { + return parent_->trajectory_nodes_; +} + +std::vector +PoseGraph3D::TrimmingHandle::GetConstraints() const { + return parent_->constraints_; +} + 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 da00050..2d76997 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -288,6 +288,8 @@ class PoseGraph3D : public PoseGraph { std::vector GetSubmapIds(int trajectory_id) const override; MapById GetAllSubmapData() const override; + MapById GetTrajectoryNodes() const override; + std::vector GetConstraints() 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.h b/cartographer/mapping/pose_graph_trimmer.h index 82a05f5..08c7ff9 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/cartographer/mapping/pose_graph_trimmer.h @@ -34,6 +34,9 @@ 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() + const = 0; // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They // will no longer take part in scan matching, loop closure, visualization. diff --git a/cartographer/mapping/pose_graph_trimmer_test.cc b/cartographer/mapping/pose_graph_trimmer_test.cc index 6bdcd0f..d780f6d 100644 --- a/cartographer/mapping/pose_graph_trimmer_test.cc +++ b/cartographer/mapping/pose_graph_trimmer_test.cc @@ -47,6 +47,14 @@ class FakePoseGraph : public Trimmable { return {}; } + MapById GetTrajectoryNodes() const override { + return {}; + } + + std::vector GetConstraints() const override { + return {}; + } + void MarkSubmapAsTrimmed(const SubmapId& submap_id) override { trimmed_submaps_.push_back(submap_id); }