From ce18ec72950974cf9ed7e818ee0693ed4ed05308 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Fri, 25 May 2018 12:29:52 +0200 Subject: [PATCH] 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. --- .../cloud/internal/client/pose_graph_stub.cc | 5 +++ .../cloud/internal/client/pose_graph_stub.h | 2 ++ .../cloud/internal/client_server_test.cc | 7 ++-- cartographer/cloud/map_builder_server_main.cc | 3 +- .../mapping/internal/2d/pose_graph_2d.cc | 7 ++-- .../mapping/internal/2d/pose_graph_2d.h | 3 +- .../mapping/internal/2d/pose_graph_2d_test.cc | 2 +- .../mapping/internal/3d/pose_graph_3d.cc | 7 ++-- .../mapping/internal/3d/pose_graph_3d.h | 3 +- .../mapping/internal/3d/pose_graph_3d_test.cc | 14 ++++---- .../internal/testing/mock_map_builder.h | 1 + .../internal/testing/mock_pose_graph.h | 2 ++ cartographer/mapping/map_builder.cc | 8 ++--- cartographer/mapping/map_builder.h | 34 +++++++++---------- cartographer/mapping/map_builder_test.cc | 3 +- cartographer/mapping/pose_graph.h | 4 --- cartographer/mapping/pose_graph_interface.h | 9 +++++ 17 files changed, 64 insertions(+), 50 deletions(-) diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 6765752..33e15f7 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/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 diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index 52d4040..9fa2de1 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -50,6 +50,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { const override; std::vector constraints() const override; mapping::proto::PoseGraph ToProto() const override; + void SetGlobalSlamOptimizationCallback( + GlobalSlamOptimizationCallback callback) override; private: std::shared_ptr<::grpc::Channel> client_channel_; diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index df1ed48..0cb0093 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -109,9 +109,7 @@ class ClientServerTest : public ::testing::Test { void InitializeRealServer() { auto map_builder = common::make_unique( - map_builder_server_options_.map_builder_options(), - nullptr /* global_slam_optimization_callback */ - ); + map_builder_server_options_.map_builder_options()); server_ = common::make_unique(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( - uploading_map_builder_server_options_.map_builder_options(), - nullptr /* global_slam_optimization_callback */); + uploading_map_builder_server_options_.map_builder_options()); uploading_server_ = common::make_unique( uploading_map_builder_server_options_, std::move(map_builder)); EXPECT_TRUE(uploading_server_ != nullptr); diff --git a/cartographer/cloud/map_builder_server_main.cc b/cartographer/cloud/map_builder_server_main.cc index 163e139..054989c 100644 --- a/cartographer/cloud/map_builder_server_main.cc +++ b/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( - map_builder_server_options.map_builder_options(), - nullptr /* global_slam_optimization_callback */); + map_builder_server_options.map_builder_options()); std::unique_ptr map_builder_server = CreateMapBuilderServer(map_builder_server_options, std::move(map_builder)); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index d9a88b7..be02136 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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_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 diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index c271609..83c8a71 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/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_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_); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 7704e4b..bceb588 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/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( - options, nullptr /* global_slam_optimization_callback */, + options, common::make_unique( options.optimization_problem_options()), &thread_pool_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 0a39366..ed7e6b0 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/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_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 diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index b8b9c0d..24ce57e 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/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_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_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index fd6cca7..286437e 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/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&, const std::set&, - const std::map&)); + MOCK_METHOD3(Solve, + void(const std::vector &, const std::set &, + const std::map &)); }; class PoseGraph3DForTesting : public PoseGraph3D { public: PoseGraph3DForTesting( - const proto::PoseGraphOptions& options, + const proto::PoseGraphOptions &options, std::unique_ptr optimization_problem, - common::ThreadPool* thread_pool) - : PoseGraph3D(options, nullptr /* global_slam_optimization_callback */, - std::move(optimization_problem), thread_pool) {} + common::ThreadPool *thread_pool) + : PoseGraph3D(options, 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) { diff --git a/cartographer/mapping/internal/testing/mock_map_builder.h b/cartographer/mapping/internal/testing/mock_map_builder.h index 92f2412..78e8af9 100644 --- a/cartographer/mapping/internal/testing/mock_map_builder.h +++ b/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" diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 6150409..7a003dd 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -54,6 +54,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface { std::map()); MOCK_CONST_METHOD0(constraints, std::vector()); MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph()); + MOCK_METHOD1(SetGlobalSlamOptimizationCallback, + void(GlobalSlamOptimizationCallback callback)); }; } // namespace testing diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index d33b0a3..ec00b52 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -61,22 +61,20 @@ std::vector SelectRangeSensorIds( return range_sensor_ids; } -MapBuilder::MapBuilder( - const proto::MapBuilderOptions& options, - PoseGraph::GlobalSlamOptimizationCallback global_slam_optimization_callback) +MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : 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( - options_.pose_graph_options(), global_slam_optimization_callback, + options_.pose_graph_options(), common::make_unique( options_.pose_graph_options().optimization_problem_options()), &thread_pool_); } if (options.use_trajectory_builder_3d()) { pose_graph_ = common::make_unique( - options_.pose_graph_options(), global_slam_optimization_callback, + options_.pose_graph_options(), common::make_unique( options_.pose_graph_options().optimization_problem_options()), &thread_pool_); diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 886f0a2..880f3bd 100644 --- a/cartographer/mapping/map_builder.h +++ b/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, - PoseGraph::GlobalSlamOptimizationCallback - global_slam_optimization_callback); + explicit MapBuilder(const proto::MapBuilderOptions &options); ~MapBuilder() override {} - MapBuilder(const MapBuilder&) = delete; - MapBuilder& operator=(const MapBuilder&) = delete; + MapBuilder(const MapBuilder &) = delete; + MapBuilder &operator=(const MapBuilder &) = delete; int AddTrajectoryBuilder( - const std::set& expected_sensor_ids, - const proto::TrajectoryBuilderOptions& trajectory_options, + const std::set &expected_sensor_ids, + const proto::TrajectoryBuilderOptions &trajectory_options, LocalSlamResultCallback local_slam_result_callback) override; int AddTrajectoryForDeserialization( - const proto::TrajectoryBuilderOptionsWithSensorIds& - options_with_sensor_ids_proto) override; + const proto::TrajectoryBuilderOptionsWithSensorIds + &options_with_sensor_ids_proto) override; void FinishTrajectory(int trajectory_id) override; - std::string SubmapToProto(const SubmapId& submap_id, - proto::SubmapQuery::Response* response) override; + std::string SubmapToProto(const SubmapId &submap_id, + 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& - GetAllTrajectoryBuilderOptions() const override { + const std::vector + &GetAllTrajectoryBuilderOptions() const override { return all_trajectory_builder_options_; } diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index aa77203..3c29164 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -58,8 +58,7 @@ class MapBuilderTest : public ::testing::Test { } void BuildMapBuilder() { - map_builder_ = common::make_unique( - map_builder_options_, nullptr /* global_slam_optimization_callback */); + map_builder_ = common::make_unique(map_builder_options_); } void SetOptionsTo3D() { diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 833582b..80c5993 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -52,10 +52,6 @@ class PoseGraph : public PoseGraphInterface { common::Time time; }; - using GlobalSlamOptimizationCallback = - std::function&, - const std::map&)>; - PoseGraph() {} ~PoseGraph() override {} diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 5642185..6483ef4 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -79,6 +79,10 @@ class PoseGraphInterface { common::optional fixed_frame_origin_in_map; }; + using GlobalSlamOptimizationCallback = + std::function&, + const std::map&)>; + 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