Add SetGlobalSlamOptimizationCallback() (#1164)

to allow setting the GlobalSlamOptimizationCallback after MapBuilder and PoseGraph
creation. Also removes the GlobalSlamOptimizationCallback from the Constructor
since it is not used.

Prerequisite for implementing ReceiveGlobalSlamOptimizations() in gRPC MapBuilderInterface.

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -158,5 +158,10 @@ mapping::proto::PoseGraph PoseGraphStub::ToProto() const {
   LOG(FATAL) << "Not implemented";
 }
 
+void PoseGraphStub::SetGlobalSlamOptimizationCallback(
+    GlobalSlamOptimizationCallback callback) {
+  LOG(FATAL) << "Not implemented";
+}
+
 }  // namespace cloud
 }  // namespace cartographer

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -50,6 +50,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
       const override;
   std::vector<Constraint> constraints() const override;
   mapping::proto::PoseGraph ToProto() const override;
+  void SetGlobalSlamOptimizationCallback(
+      GlobalSlamOptimizationCallback callback) override;
 
  private:
   std::shared_ptr<::grpc::Channel> client_channel_;

cartographer/cloud/internal/client_server_test.cc

@@ -109,9 +109,7 @@ 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);
@@ -119,8 +117,7 @@ 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,8 +55,7 @@ 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,11 +41,9 @@ 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) {}
 
@@ -928,5 +926,10 @@ PoseGraph2D::GetSubmapDataUnderLock() const {
   return submaps;
 }
 
+void PoseGraph2D::SetGlobalSlamOptimizationCallback(
+    PoseGraphInterface::GlobalSlamOptimizationCallback callback) {
+  global_slam_optimization_callback_ = callback;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

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

@@ -61,7 +61,6 @@ 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;
@@ -141,6 +140,8 @@ class PoseGraph2D : public PoseGraph {
                                 const transform::Rigid3d& pose,
                                 const common::Time time) override
       EXCLUDES(mutex_);
+  void SetGlobalSlamOptimizationCallback(
+      PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
   transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
       int trajectory_id, const common::Time time) const REQUIRES(mutex_);
 

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, nullptr /* global_slam_optimization_callback */,
+          options,
           common::make_unique<optimization::OptimizationProblem2D>(
               options.optimization_problem_options()),
           &thread_pool_);

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

@@ -41,11 +41,9 @@ 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) {}
 
@@ -956,5 +954,10 @@ PoseGraph3D::GetSubmapDataUnderLock() const {
   return submaps;
 }
 
+void PoseGraph3D::SetGlobalSlamOptimizationCallback(
+    PoseGraphInterface::GlobalSlamOptimizationCallback callback) {
+  global_slam_optimization_callback_ = callback;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

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

@@ -60,7 +60,6 @@ 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;
@@ -140,6 +139,8 @@ class PoseGraph3D : public PoseGraph {
                                 const transform::Rigid3d& pose,
                                 const common::Time time) override
       EXCLUDES(mutex_);
+  void SetGlobalSlamOptimizationCallback(
+      PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
   transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
       int trajectory_id, const common::Time time) const REQUIRES(mutex_);
 

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

@@ -38,18 +38,18 @@ class MockOptimizationProblem3D : public OptimizationProblem3D {
       : OptimizationProblem3D(OptimizationProblemOptions{}) {}
   ~MockOptimizationProblem3D() override = default;
 
-  MOCK_METHOD3(Solve, void(const std::vector<Constraint>&, const std::set<int>&,
+  MOCK_METHOD3(Solve,
-                           const std::map<std::string, LandmarkNode>&));
+               void(const std::vector<Constraint> &, const std::set<int> &,
+                    const std::map<std::string, LandmarkNode> &));
 };
 
 class PoseGraph3DForTesting : public PoseGraph3D {
  public:
   PoseGraph3DForTesting(
-      const proto::PoseGraphOptions& options,
+      const proto::PoseGraphOptions &options,
       std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
-      common::ThreadPool* thread_pool)
+      common::ThreadPool *thread_pool)
-      : PoseGraph3D(options, nullptr /* global_slam_optimization_callback */,
+      : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
-                    std::move(optimization_problem), thread_pool) {}
 
   void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
 };
@@ -199,7 +199,7 @@ class EvenSubmapTrimmer : public PoseGraphTrimmer {
   explicit EvenSubmapTrimmer(int trajectory_id)
       : trajectory_id_(trajectory_id) {}
 
-  void Trim(Trimmable* pose_graph) override {
+  void Trim(Trimmable *pose_graph) override {
     auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
     for (const auto submap_id : submap_ids) {
       if (submap_id.submap_index % 2 == 0) {

cartographer/mapping/internal/testing/mock_map_builder.h

@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_MAP_BUILDER_H_
 
 #include "cartographer/mapping/map_builder_interface.h"
+#include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "glog/logging.h"
 #include "gmock/gmock.h"

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -54,6 +54,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
       std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
   MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
   MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
+  MOCK_METHOD1(SetGlobalSlamOptimizationCallback,
+               void(GlobalSlamOptimizationCallback callback));
 };
 
 }  // namespace testing

cartographer/mapping/map_builder.cc

@@ -61,22 +61,20 @@ std::vector<std::string> SelectRangeSensorIds(
   return range_sensor_ids;
 }
 
-MapBuilder::MapBuilder(
+MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
-    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(), global_slam_optimization_callback,
+        options_.pose_graph_options(),
         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(), global_slam_optimization_callback,
+        options_.pose_graph_options(),
         common::make_unique<optimization::OptimizationProblem3D>(
             options_.pose_graph_options().optimization_problem_options()),
         &thread_pool_);

cartographer/mapping/map_builder.h

@@ -30,40 +30,38 @@ namespace cartographer {
 namespace mapping {
 
 proto::MapBuilderOptions CreateMapBuilderOptions(
-    common::LuaParameterDictionary* const parameter_dictionary);
+    common::LuaParameterDictionary *const parameter_dictionary);
 
 // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
 // and a PoseGraph for loop closure.
 class MapBuilder : public MapBuilderInterface {
  public:
-  MapBuilder(const proto::MapBuilderOptions& options,
+  explicit MapBuilder(const proto::MapBuilderOptions &options);
-             PoseGraph::GlobalSlamOptimizationCallback
-                 global_slam_optimization_callback);
   ~MapBuilder() override {}
 
-  MapBuilder(const MapBuilder&) = delete;
+  MapBuilder(const MapBuilder &) = delete;
-  MapBuilder& operator=(const MapBuilder&) = delete;
+  MapBuilder &operator=(const MapBuilder &) = delete;
 
   int AddTrajectoryBuilder(
-      const std::set<SensorId>& expected_sensor_ids,
+      const std::set<SensorId> &expected_sensor_ids,
-      const proto::TrajectoryBuilderOptions& trajectory_options,
+      const proto::TrajectoryBuilderOptions &trajectory_options,
       LocalSlamResultCallback local_slam_result_callback) override;
 
   int AddTrajectoryForDeserialization(
-      const proto::TrajectoryBuilderOptionsWithSensorIds&
+      const proto::TrajectoryBuilderOptionsWithSensorIds
-          options_with_sensor_ids_proto) override;
+          &options_with_sensor_ids_proto) override;
 
   void FinishTrajectory(int trajectory_id) override;
 
-  std::string SubmapToProto(const SubmapId& submap_id,
+  std::string SubmapToProto(const SubmapId &submap_id,
-                            proto::SubmapQuery::Response* response) override;
+                            proto::SubmapQuery::Response *response) override;
 
-  void SerializeState(io::ProtoStreamWriterInterface* writer) override;
+  void SerializeState(io::ProtoStreamWriterInterface *writer) override;
 
-  void LoadState(io::ProtoStreamReaderInterface* reader,
+  void LoadState(io::ProtoStreamReaderInterface *reader,
                  bool load_frozen_state) override;
 
-  mapping::PoseGraphInterface* pose_graph() override {
+  mapping::PoseGraphInterface *pose_graph() override {
     return pose_graph_.get();
   }
 
@@ -71,13 +69,13 @@ class MapBuilder : public MapBuilderInterface {
     return trajectory_builders_.size();
   }
 
-  mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
+  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
       int trajectory_id) const override {
     return trajectory_builders_.at(trajectory_id).get();
   }
 
-  const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
+  const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
-  GetAllTrajectoryBuilderOptions() const override {
+      &GetAllTrajectoryBuilderOptions() const override {
     return all_trajectory_builder_options_;
   }
 

cartographer/mapping/map_builder_test.cc

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

cartographer/mapping/pose_graph.h

@@ -52,10 +52,6 @@ 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 {}
 

cartographer/mapping/pose_graph_interface.h

@@ -79,6 +79,10 @@ class PoseGraphInterface {
     common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
   };
 
+  using GlobalSlamOptimizationCallback =
+      std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
+                         const std::map<int /* trajectory_id */, NodeId>&)>;
+
   PoseGraphInterface() {}
   virtual ~PoseGraphInterface() {}
 
@@ -129,6 +133,11 @@ class PoseGraphInterface {
 
   // Serializes the constraints and trajectories.
   virtual proto::PoseGraph ToProto() const = 0;
+
+  // Sets the callback function that is invoked whenever the global optimization
+  // problem is solved.
+  virtual void SetGlobalSlamOptimizationCallback(
+      GlobalSlamOptimizationCallback callback) = 0;
 };
 
 }  // namespace mapping