Perform final optimization at the end. (#483)

cartographer_ros/cartographer_ros/node.cc

@@ -428,7 +428,6 @@ bool Node::HandleWriteState(
 }
 
 void Node::FinishAllTrajectories() {
-  {
   carto::common::MutexLocker lock(&mutex_);
   for (auto& entry : is_active_trajectory_) {
     const int trajectory_id = entry.first;
@@ -437,17 +436,24 @@ void Node::FinishAllTrajectories() {
       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_);
+    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();
 }
 

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);
 

cartographer_ros/cartographer_ros/node_main.cc

@@ -50,6 +50,7 @@ void Run() {
   ::ros::spin();
 
   node.FinishAllTrajectories();
+  node.RunFinalOptimization();
 }
 
 }  // namespace

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();