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,7 +38,8 @@ class MockOptimizationProblem3D : public OptimizationProblem3D {
       : OptimizationProblem3D(OptimizationProblemOptions{}) {}
   ~MockOptimizationProblem3D() override = default;
 
-  MOCK_METHOD3(Solve, void(const std::vector<Constraint>&, const std::set<int>&,
+  MOCK_METHOD3(Solve,
+               void(const std::vector<Constraint> &, const std::set<int> &,
                     const std::map<std::string, LandmarkNode> &));
 };
 
@@ -48,8 +49,7 @@ class PoseGraph3DForTesting : public PoseGraph3D {
       const proto::PoseGraphOptions &options,
       std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
       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(); }
 };

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

@@ -36,9 +36,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
 // 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;
@@ -50,8 +48,8 @@ class MapBuilder : public MapBuilderInterface {
       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;
 
@@ -76,8 +74,8 @@ class MapBuilder : public MapBuilderInterface {
     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