Perform final optimization at the end. (#483)

cartographer_ros/cartographer_ros/node.cc

@@ -428,26 +428,32 @@ bool Node::HandleWriteState(
 }
 
 void Node::FinishAllTrajectories() {
-  {
+  carto::common::MutexLocker lock(&mutex_);
-    carto::common::MutexLocker lock(&mutex_);
+  for (auto& entry : is_active_trajectory_) {
-    for (auto& entry : is_active_trajectory_) {
+    const int trajectory_id = entry.first;
-      const int trajectory_id = entry.first;
+    if (entry.second) {
-      if (entry.second) {
+      map_builder_bridge_.FinishTrajectory(trajectory_id);
-        map_builder_bridge_.FinishTrajectory(trajectory_id);
+      entry.second = false;
-        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));
+    for (const auto& entry : is_active_trajectory_) {
-    map_builder_bridge_.FinishTrajectory(trajectory_id);
+      CHECK(!entry.second);
-    is_active_trajectory_[trajectory_id] = false;
+    }
   }
+  // 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();