Introduce a GlobalSlamResultCallback (#1143)

cartographer/cloud/internal/client_server_test.cc

@@ -109,7 +109,9 @@ class ClientServerTest : public ::testing::Test {
 
   void InitializeRealServer() {
     auto map_builder = common::make_unique<MapBuilder>(
-        map_builder_server_options_.map_builder_options());
+        map_builder_server_options_.map_builder_options(),
+        nullptr /* global_slam_optimization_callback */
+    );
     server_ = common::make_unique<MapBuilderServer>(map_builder_server_options_,
                                                     std::move(map_builder));
     EXPECT_TRUE(server_ != nullptr);
@@ -117,7 +119,8 @@ class ClientServerTest : public ::testing::Test {
 
   void InitializeRealUploadingServer() {
     auto map_builder = common::make_unique<MapBuilder>(
-        uploading_map_builder_server_options_.map_builder_options());
+        uploading_map_builder_server_options_.map_builder_options(),
+        nullptr /* global_slam_optimization_callback */);
     uploading_server_ = common::make_unique<MapBuilderServer>(
         uploading_map_builder_server_options_, std::move(map_builder));
     EXPECT_TRUE(uploading_server_ != nullptr);

cartographer/cloud/map_builder_server_main.cc

@@ -55,7 +55,8 @@ void Run(const std::string& configuration_directory,
   map_builder_server_options.mutable_map_builder_options()
       ->set_collate_by_trajectory(true);
   auto map_builder = common::make_unique<mapping::MapBuilder>(
-      map_builder_server_options.map_builder_options());
+      map_builder_server_options.map_builder_options(),
+      nullptr /* global_slam_optimization_callback */);
   std::unique_ptr<MapBuilderServerInterface> map_builder_server =
       CreateMapBuilderServer(map_builder_server_options,
                              std::move(map_builder));

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -41,9 +41,11 @@ namespace mapping {
 
 PoseGraph2D::PoseGraph2D(
     const proto::PoseGraphOptions& options,
+    GlobalSlamOptimizationCallback global_slam_optimization_callback,
     std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
     common::ThreadPool* thread_pool)
     : options_(options),
+      global_slam_optimization_callback_(global_slam_optimization_callback),
       optimization_problem_(std::move(optimization_problem)),
       constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
 
@@ -301,7 +303,8 @@ void PoseGraph2D::DispatchOptimization() {
   // If there is a 'work_queue_' already, some other thread will take care.
   if (work_queue_ == nullptr) {
     work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
-    HandleWorkQueue();
+    constraint_builder_.WhenDone(
+        std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
   }
 }
 common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
@@ -326,44 +329,62 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
                                          time);
 }
 
-void PoseGraph2D::HandleWorkQueue() {
+void PoseGraph2D::HandleWorkQueue(
+    const constraints::ConstraintBuilder2D::Result& result) {
+  {
+    common::MutexLocker locker(&mutex_);
+    constraints_.insert(constraints_.end(), result.begin(), result.end());
+  }
+  RunOptimization();
+
+  if (global_slam_optimization_callback_) {
+    std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
+    std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
+    {
+      common::MutexLocker locker(&mutex_);
+      const auto& submap_data = optimization_problem_->submap_data();
+      const auto& node_data = optimization_problem_->node_data();
+      for (const int trajectory_id : node_data.trajectory_ids()) {
+        trajectory_id_to_last_optimized_node_id[trajectory_id] =
+            std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
+        trajectory_id_to_last_optimized_submap_id[trajectory_id] =
+            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
+      }
+    }
+    global_slam_optimization_callback_(
+        trajectory_id_to_last_optimized_submap_id,
+        trajectory_id_to_last_optimized_node_id);
+  }
+
+  common::MutexLocker locker(&mutex_);
+  for (const Constraint& constraint : result) {
+    UpdateTrajectoryConnectivity(constraint);
+  }
+  TrimmingHandle trimming_handle(this);
+  for (auto& trimmer : trimmers_) {
+    trimmer->Trim(&trimming_handle);
+  }
+  trimmers_.erase(
+      std::remove_if(trimmers_.begin(), trimmers_.end(),
+                     [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
+                       return trimmer->IsFinished();
+                     }),
+      trimmers_.end());
+
+  num_nodes_since_last_loop_closure_ = 0;
+  run_loop_closure_ = false;
+  while (!run_loop_closure_) {
+    if (work_queue_->empty()) {
+      work_queue_.reset();
+      return;
+    }
+    work_queue_->front()();
+    work_queue_->pop_front();
+  }
+  LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
+  // We have to optimize again.
   constraint_builder_.WhenDone(
-      [this](const constraints::ConstraintBuilder2D::Result& result) {
+      std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
-        {
-          common::MutexLocker locker(&mutex_);
-          constraints_.insert(constraints_.end(), result.begin(), result.end());
-        }
-        RunOptimization();
-
-        common::MutexLocker locker(&mutex_);
-        for (const Constraint& constraint : result) {
-          UpdateTrajectoryConnectivity(constraint);
-        }
-        TrimmingHandle trimming_handle(this);
-        for (auto& trimmer : trimmers_) {
-          trimmer->Trim(&trimming_handle);
-        }
-        trimmers_.erase(
-            std::remove_if(trimmers_.begin(), trimmers_.end(),
-                           [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
-                             return trimmer->IsFinished();
-                           }),
-            trimmers_.end());
-
-        num_nodes_since_last_loop_closure_ = 0;
-        run_loop_closure_ = false;
-        while (!run_loop_closure_) {
-          if (work_queue_->empty()) {
-            work_queue_.reset();
-            return;
-          }
-          work_queue_->front()();
-          work_queue_->pop_front();
-        }
-        LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
-        // We have to optimize again.
-        HandleWorkQueue();
-      });
 }
 
 void PoseGraph2D::WaitForAllComputations() {

cartographer/mapping/internal/2d/pose_graph_2d.h

@@ -61,6 +61,7 @@ class PoseGraph2D : public PoseGraph {
  public:
   PoseGraph2D(
       const proto::PoseGraphOptions& options,
+      GlobalSlamOptimizationCallback global_slam_optimization_callback,
       std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
       common::ThreadPool* thread_pool);
   ~PoseGraph2D() override;
@@ -186,9 +187,9 @@ class PoseGraph2D : public PoseGraph {
   void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
       REQUIRES(mutex_);
 
-  // Registers the callback to run the optimization once all constraints have
+  // Runs the optimization, executes the trimmers and processes the work queue.
-  // been computed, that will also do all work that queue up in 'work_queue_'.
+  void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
-  void HandleWorkQueue() REQUIRES(mutex_);
+      REQUIRES(mutex_);
 
   // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
   // all computations have finished.
@@ -215,6 +216,7 @@ class PoseGraph2D : public PoseGraph {
       REQUIRES(mutex_);
 
   const proto::PoseGraphOptions options_;
+  GlobalSlamOptimizationCallback global_slam_optimization_callback_;
   common::Mutex mutex_;
 
   // If it exists, further work items must be added to this queue, and will be

cartographer/mapping/internal/2d/pose_graph_2d_test.cc

@@ -142,7 +142,7 @@ class PoseGraph2DTest : public ::testing::Test {
           })text");
       auto options = CreatePoseGraphOptions(parameter_dictionary.get());
       pose_graph_ = common::make_unique<PoseGraph2D>(
-          options,
+          options, nullptr /* global_slam_optimization_callback */,
           common::make_unique<optimization::OptimizationProblem2D>(
               options.optimization_problem_options()),
           &thread_pool_);

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -41,9 +41,11 @@ namespace mapping {
 
 PoseGraph3D::PoseGraph3D(
     const proto::PoseGraphOptions& options,
+    GlobalSlamOptimizationCallback global_slam_optimization_callback,
     std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
     common::ThreadPool* thread_pool)
     : options_(options),
+      global_slam_optimization_callback_(global_slam_optimization_callback),
       optimization_problem_(std::move(optimization_problem)),
       constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
 
@@ -315,7 +317,8 @@ void PoseGraph3D::DispatchOptimization() {
   // If there is a 'work_queue_' already, some other thread will take care.
   if (work_queue_ == nullptr) {
     work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
-    HandleWorkQueue();
+    constraint_builder_.WhenDone(
+        std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1));
   }
 }
 
@@ -341,44 +344,62 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
                                          time);
 }
 
-void PoseGraph3D::HandleWorkQueue() {
+void PoseGraph3D::HandleWorkQueue(
+    const constraints::ConstraintBuilder3D::Result& result) {
+  {
+    common::MutexLocker locker(&mutex_);
+    constraints_.insert(constraints_.end(), result.begin(), result.end());
+  }
+  RunOptimization();
+
+  if (global_slam_optimization_callback_) {
+    std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
+    std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
+    {
+      common::MutexLocker locker(&mutex_);
+      const auto& submap_data = optimization_problem_->submap_data();
+      const auto& node_data = optimization_problem_->node_data();
+      for (const int trajectory_id : node_data.trajectory_ids()) {
+        trajectory_id_to_last_optimized_node_id[trajectory_id] =
+            std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
+        trajectory_id_to_last_optimized_submap_id[trajectory_id] =
+            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
+      }
+    }
+    global_slam_optimization_callback_(
+        trajectory_id_to_last_optimized_submap_id,
+        trajectory_id_to_last_optimized_node_id);
+  }
+
+  common::MutexLocker locker(&mutex_);
+  for (const Constraint& constraint : result) {
+    UpdateTrajectoryConnectivity(constraint);
+  }
+  TrimmingHandle trimming_handle(this);
+  for (auto& trimmer : trimmers_) {
+    trimmer->Trim(&trimming_handle);
+  }
+  trimmers_.erase(
+      std::remove_if(trimmers_.begin(), trimmers_.end(),
+                     [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
+                       return trimmer->IsFinished();
+                     }),
+      trimmers_.end());
+
+  num_nodes_since_last_loop_closure_ = 0;
+  run_loop_closure_ = false;
+  while (!run_loop_closure_) {
+    if (work_queue_->empty()) {
+      work_queue_.reset();
+      return;
+    }
+    work_queue_->front()();
+    work_queue_->pop_front();
+  }
+  LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
+  // We have to optimize again.
   constraint_builder_.WhenDone(
-      [this](const constraints::ConstraintBuilder3D::Result& result) {
+      std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1));
-        {
-          common::MutexLocker locker(&mutex_);
-          constraints_.insert(constraints_.end(), result.begin(), result.end());
-        }
-        RunOptimization();
-
-        common::MutexLocker locker(&mutex_);
-        for (const Constraint& constraint : result) {
-          UpdateTrajectoryConnectivity(constraint);
-        }
-        TrimmingHandle trimming_handle(this);
-        for (auto& trimmer : trimmers_) {
-          trimmer->Trim(&trimming_handle);
-        }
-        trimmers_.erase(
-            std::remove_if(trimmers_.begin(), trimmers_.end(),
-                           [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
-                             return trimmer->IsFinished();
-                           }),
-            trimmers_.end());
-
-        num_nodes_since_last_loop_closure_ = 0;
-        run_loop_closure_ = false;
-        while (!run_loop_closure_) {
-          if (work_queue_->empty()) {
-            work_queue_.reset();
-            return;
-          }
-          work_queue_->front()();
-          work_queue_->pop_front();
-        }
-        LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
-        // We have to optimize again.
-        HandleWorkQueue();
-      });
 }
 
 void PoseGraph3D::WaitForAllComputations() {

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -60,6 +60,7 @@ class PoseGraph3D : public PoseGraph {
  public:
   PoseGraph3D(
       const proto::PoseGraphOptions& options,
+      GlobalSlamOptimizationCallback global_slam_optimization_callback,
       std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
       common::ThreadPool* thread_pool);
   ~PoseGraph3D() override;
@@ -189,9 +190,9 @@ class PoseGraph3D : public PoseGraph {
   void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
       REQUIRES(mutex_);
 
-  // Registers the callback to run the optimization once all constraints have
+  // Runs the optimization, executes the trimmers and processes the work queue.
-  // been computed, that will also do all work that queue up in 'work_queue_'.
+  void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result)
-  void HandleWorkQueue() REQUIRES(mutex_);
+      REQUIRES(mutex_);
 
   // Runs the optimization. Callers have to make sure, that there is only one
   // optimization being run at a time.
@@ -219,6 +220,7 @@ class PoseGraph3D : public PoseGraph {
       REQUIRES(mutex_);
 
   const proto::PoseGraphOptions options_;
+  GlobalSlamOptimizationCallback global_slam_optimization_callback_;
   common::Mutex mutex_;
 
   // If it exists, further work items must be added to this queue, and will be

cartographer/mapping/internal/3d/pose_graph_3d_test.cc

@@ -48,7 +48,8 @@ class PoseGraph3DForTesting : public PoseGraph3D {
       const proto::PoseGraphOptions& options,
       std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
       common::ThreadPool* thread_pool)
-      : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
+      : PoseGraph3D(options, nullptr /* global_slam_optimization_callback */,
+                    std::move(optimization_problem), thread_pool) {}
 
   void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
 };

cartographer/mapping/map_builder.cc

@@ -61,20 +61,22 @@ std::vector<std::string> SelectRangeSensorIds(
   return range_sensor_ids;
 }
 
-MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
+MapBuilder::MapBuilder(
+    const proto::MapBuilderOptions& options,
+    PoseGraph::GlobalSlamOptimizationCallback global_slam_optimization_callback)
     : options_(options), thread_pool_(options.num_background_threads()) {
   CHECK(options.use_trajectory_builder_2d() ^
         options.use_trajectory_builder_3d());
   if (options.use_trajectory_builder_2d()) {
     pose_graph_ = common::make_unique<PoseGraph2D>(
-        options_.pose_graph_options(),
+        options_.pose_graph_options(), global_slam_optimization_callback,
         common::make_unique<optimization::OptimizationProblem2D>(
             options_.pose_graph_options().optimization_problem_options()),
         &thread_pool_);
   }
   if (options.use_trajectory_builder_3d()) {
     pose_graph_ = common::make_unique<PoseGraph3D>(
-        options_.pose_graph_options(),
+        options_.pose_graph_options(), global_slam_optimization_callback,
         common::make_unique<optimization::OptimizationProblem3D>(
             options_.pose_graph_options().optimization_problem_options()),
         &thread_pool_);

cartographer/mapping/map_builder.h

@@ -36,7 +36,9 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
 // and a PoseGraph for loop closure.
 class MapBuilder : public MapBuilderInterface {
  public:
-  explicit MapBuilder(const proto::MapBuilderOptions& options);
+  MapBuilder(const proto::MapBuilderOptions& options,
+             PoseGraph::GlobalSlamOptimizationCallback
+                 global_slam_optimization_callback);
   ~MapBuilder() override {}
 
   MapBuilder(const MapBuilder&) = delete;

cartographer/mapping/map_builder_test.cc

@@ -58,7 +58,8 @@ class MapBuilderTest : public ::testing::Test {
   }
 
   void BuildMapBuilder() {
-    map_builder_ = common::make_unique<MapBuilder>(map_builder_options_);
+    map_builder_ = common::make_unique<MapBuilder>(
+        map_builder_options_, nullptr /* global_slam_optimization_callback */);
   }
 
   void SetOptionsTo3D() {

cartographer/mapping/pose_graph.h

@@ -52,6 +52,10 @@ class PoseGraph : public PoseGraphInterface {
     common::Time time;
   };
 
+  using GlobalSlamOptimizationCallback =
+      std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
+                         const std::map<int /* trajectory_id */, NodeId>&)>;
+
   PoseGraph() {}
   ~PoseGraph() override {}