From abb2661b7634082156f82724d7f83535d7835b16 Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Wed, 4 Apr 2018 12:50:12 +0200
Subject: [PATCH] Allow trimmer to get nodes/constraints. (#1034)

* Allow trimmer to get nodes/constraints.

* Mark the methods const.
---
 cartographer/mapping/internal/2d/pose_graph_2d.cc | 10 ++++++++++
 cartographer/mapping/internal/2d/pose_graph_2d.h  |  2 ++
 cartographer/mapping/internal/3d/pose_graph_3d.cc | 10 ++++++++++
 cartographer/mapping/internal/3d/pose_graph_3d.h  |  2 ++
 cartographer/mapping/pose_graph_trimmer.h         |  3 +++
 cartographer/mapping/pose_graph_trimmer_test.cc   |  8 ++++++++
 6 files changed, 35 insertions(+)

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<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
   return submap_ids;
 }
 
+MapById<NodeId, TrajectoryNode>
+PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
+  return parent_->trajectory_nodes_;
+}
+
+std::vector<PoseGraphInterface::Constraint>
+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<SubmapId> GetSubmapIds(int trajectory_id) const override;
     MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
         const override;
+    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override;
+    std::vector<PoseGraphInterface::Constraint> 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<NodeId, TrajectoryNode>
+PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const {
+  return parent_->trajectory_nodes_;
+}
+
+std::vector<PoseGraphInterface::Constraint>
+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<SubmapId> GetSubmapIds(int trajectory_id) const override;
     MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
         const override;
+    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override;
+    std::vector<PoseGraphInterface::Constraint> 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<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
   virtual MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
       const = 0;
+  virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0;
+  virtual std::vector<PoseGraphInterface::Constraint> 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<NodeId, TrajectoryNode> GetTrajectoryNodes() const override {
+    return {};
+  }
+
+  std::vector<PoseGraphInterface::Constraint> GetConstraints() const override {
+    return {};
+  }
+
   void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
     trimmed_submaps_.push_back(submap_id);
   }