From 3bdee588bd62d28298c44d4e259c03799f2bd4b7 Mon Sep 17 00:00:00 2001
From: Jihoon Lee <jihoonlee.in@gmail.com>
Date: Thu, 9 Nov 2017 14:29:26 +0100
Subject: [PATCH] Add FinishTrajectory() in SparsePosegraph. (#562)

---
 cartographer/mapping/map_builder.cc          | 1 +
 cartographer/mapping/sparse_pose_graph.h     | 3 +++
 cartographer/mapping_2d/sparse_pose_graph.cc | 5 +++++
 cartographer/mapping_2d/sparse_pose_graph.h  | 1 +
 cartographer/mapping_3d/sparse_pose_graph.cc | 5 +++++
 cartographer/mapping_3d/sparse_pose_graph.h  | 1 +
 6 files changed, 16 insertions(+)

diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index 05d30dc..adbaed5 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -125,6 +125,7 @@ TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
 
 void MapBuilder::FinishTrajectory(const int trajectory_id) {
   sensor_collator_.FinishTrajectory(trajectory_id);
+  sparse_pose_graph_->FinishTrajectory(trajectory_id);
 }
 
 int MapBuilder::GetBlockingTrajectoryId() const {
diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h
index 14f866a..6a381c2 100644
--- a/cartographer/mapping/sparse_pose_graph.h
+++ b/cartographer/mapping/sparse_pose_graph.h
@@ -80,6 +80,9 @@ class SparsePoseGraph {
   SparsePoseGraph(const SparsePoseGraph&) = delete;
   SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
 
+  // Finishes the given trajectory.
+  virtual void FinishTrajectory(int trajectory_id) = 0;
+
   // Freezes a trajectory. Poses in this trajectory will not be optimized.
   virtual void FreezeTrajectory(int trajectory_id) = 0;
 
diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 35b53b1..94903e9 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -372,6 +372,11 @@ void SparsePoseGraph::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void SparsePoseGraph::FinishTrajectory(const int trajectory_id) {
+  // TODO(jihoonl): Add a logic to notify trimmers to finish the given
+  // trajectory.
+}
+
 void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   trajectory_connectivity_state_.Add(trajectory_id);
diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h
index 7d57a3a..8fb99b3 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.h
+++ b/cartographer/mapping_2d/sparse_pose_graph.h
@@ -83,6 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data);
 
+  void FinishTrajectory(int trajectory_id) override;
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(const transform::Rigid3d& global_pose,
                           const mapping::proto::Submap& submap) override;
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index cfee968..f3729d5 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -388,6 +388,11 @@ void SparsePoseGraph::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void SparsePoseGraph::FinishTrajectory(const int trajectory_id) {
+  // TODO(jihoonl): Add a logic to notify trimmers to finish the given
+  // trajectory.
+}
+
 void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   trajectory_connectivity_state_.Add(trajectory_id);
diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h
index 38d14a8..9ef7d80 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.h
+++ b/cartographer/mapping_3d/sparse_pose_graph.h
@@ -83,6 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data);
 
+  void FinishTrajectory(int trajectory_id) override;
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(const transform::Rigid3d& global_pose,
                           const mapping::proto::Submap& submap) override;