From cf13f76c8261f6c5177ef46ac1fe4db7f44b92e8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Thu, 10 Aug 2017 12:30:57 +0200
Subject: [PATCH] Perform final optimization at the end. (#483)

---
 cartographer_ros/cartographer_ros/node.cc     | 30 +++++++++++--------
 cartographer_ros/cartographer_ros/node.h      |  3 ++
 .../cartographer_ros/node_main.cc             |  1 +
 .../cartographer_ros/offline_node_main.cc     |  8 ++---
 4 files changed, 25 insertions(+), 17 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc
index 21c631e..be37763 100644
--- a/cartographer_ros/cartographer_ros/node.cc
+++ b/cartographer_ros/cartographer_ros/node.cc
@@ -428,26 +428,32 @@ bool Node::HandleWriteState(
 }
 
 void Node::FinishAllTrajectories() {
-  {
-    carto::common::MutexLocker lock(&mutex_);
-    for (auto& entry : is_active_trajectory_) {
-      const int trajectory_id = entry.first;
-      if (entry.second) {
-        map_builder_bridge_.FinishTrajectory(trajectory_id);
-        entry.second = false;
-      }
+  carto::common::MutexLocker lock(&mutex_);
+  for (auto& entry : is_active_trajectory_) {
+    const int trajectory_id = entry.first;
+    if (entry.second) {
+      map_builder_bridge_.FinishTrajectory(trajectory_id);
+      entry.second = false;
     }
   }
-  map_builder_bridge_.RunFinalOptimization();
 }
 
 void Node::FinishTrajectory(const int trajectory_id) {
+  carto::common::MutexLocker lock(&mutex_);
+  CHECK(is_active_trajectory_.at(trajectory_id));
+  map_builder_bridge_.FinishTrajectory(trajectory_id);
+  is_active_trajectory_[trajectory_id] = false;
+}
+
+void Node::RunFinalOptimization() {
   {
     carto::common::MutexLocker lock(&mutex_);
-    CHECK(is_active_trajectory_.at(trajectory_id));
-    map_builder_bridge_.FinishTrajectory(trajectory_id);
-    is_active_trajectory_[trajectory_id] = false;
+    for (const auto& entry : is_active_trajectory_) {
+      CHECK(!entry.second);
+    }
   }
+  // Assuming we are not adding new data anymore, the final optimization
+  // can be performed without holding the mutex.
   map_builder_bridge_.RunFinalOptimization();
 }
 
diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h
index 7f01d20..fc9274d 100644
--- a/cartographer_ros/cartographer_ros/node.h
+++ b/cartographer_ros/cartographer_ros/node.h
@@ -56,6 +56,9 @@ class Node {
   // Finishes a single given trajectory.
   void FinishTrajectory(int trajectory_id);
 
+  // Runs final optimization. All trajectories have to be finished when calling.
+  void RunFinalOptimization();
+
   // Starts the first trajectory with the default topics.
   void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
 
diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc
index b3930a4..aa0fed7 100644
--- a/cartographer_ros/cartographer_ros/node_main.cc
+++ b/cartographer_ros/cartographer_ros/node_main.cc
@@ -50,6 +50,7 @@ void Run() {
   ::ros::spin();
 
   node.FinishAllTrajectories();
+  node.RunFinalOptimization();
 }
 
 }  // namespace
diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc
index c582c97..a413dab 100644
--- a/cartographer_ros/cartographer_ros/offline_node_main.cc
+++ b/cartographer_ros/cartographer_ros/offline_node_main.cc
@@ -209,15 +209,13 @@ void Run(const std::vector<string>& bag_filenames) {
     }
 
     bag.close();
-    // Ensure the clock is republished also during trajectory finalization,
-    // which might take a while.
-    clock_republish_timer.start();
     node.FinishTrajectory(trajectory_id);
-    clock_republish_timer.stop();
   }
 
-  // Republish the clock after bag processing has been completed.
+  // Ensure the clock is republished after the bag has been finished, during the
+  // final optimization, serialization, and optional indefinite spinning at the end.
   clock_republish_timer.start();
+  node.RunFinalOptimization();
 
   const std::chrono::time_point<std::chrono::steady_clock> end_time =
       std::chrono::steady_clock::now();