From ce67d684babc75f7dc0f43ecb0fccd8dcbf15f18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 10 May 2018 11:25:00 +0200 Subject: [PATCH] Introduce a GlobalSlamResultCallback (#1143) --- .../cloud/internal/client_server_test.cc | 7 +- cartographer/cloud/map_builder_server_main.cc | 3 +- .../mapping/internal/2d/pose_graph_2d.cc | 97 +++++++++++-------- .../mapping/internal/2d/pose_graph_2d.h | 8 +- .../mapping/internal/2d/pose_graph_2d_test.cc | 2 +- .../mapping/internal/3d/pose_graph_3d.cc | 97 +++++++++++-------- .../mapping/internal/3d/pose_graph_3d.h | 8 +- .../mapping/internal/3d/pose_graph_3d_test.cc | 3 +- cartographer/mapping/map_builder.cc | 8 +- cartographer/mapping/map_builder.h | 4 +- cartographer/mapping/map_builder_test.cc | 3 +- cartographer/mapping/pose_graph.h | 4 + 12 files changed, 152 insertions(+), 92 deletions(-) diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 0cb0093..df1ed48 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -109,7 +109,9 @@ class ClientServerTest : public ::testing::Test { void InitializeRealServer() { auto map_builder = common::make_unique( - map_builder_server_options_.map_builder_options()); + map_builder_server_options_.map_builder_options(), + nullptr /* global_slam_optimization_callback */ + ); server_ = common::make_unique(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( - 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( 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 054989c..163e139 100644 --- a/cartographer/cloud/map_builder_server_main.cc +++ b/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( - map_builder_server_options.map_builder_options()); + map_builder_server_options.map_builder_options(), + nullptr /* global_slam_optimization_callback */); 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 73d21bd..f98a257 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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_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>>(); - 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 trajectory_id_to_last_optimized_node_id; + std::map 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& 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) { - { - 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& 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(); - }); + std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1)); } void PoseGraph2D::WaitForAllComputations() { diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 3393d9f..6844aa9 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/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_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 - // been computed, that will also do all work that queue up in 'work_queue_'. - void HandleWorkQueue() REQUIRES(mutex_); + // Runs the optimization, executes the trimmers and processes the work queue. + void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) + 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 diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index bceb588..7704e4b 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, + options, nullptr /* global_slam_optimization_callback */, 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 032e0c6..e2459ac 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/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_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>>(); - 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 trajectory_id_to_last_optimized_node_id; + std::map 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& 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) { - { - 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& 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(); - }); + std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1)); } void PoseGraph3D::WaitForAllComputations() { diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 251ecb1..e025d27 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/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_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 - // been computed, that will also do all work that queue up in 'work_queue_'. - void HandleWorkQueue() REQUIRES(mutex_); + // Runs the optimization, executes the trimmers and processes the work queue. + void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result) + 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 diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index 6f1b10b..fd6cca7 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/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_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(); } }; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index ec00b52..d33b0a3 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -61,20 +61,22 @@ std::vector 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( - options_.pose_graph_options(), + options_.pose_graph_options(), global_slam_optimization_callback, 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(), + options_.pose_graph_options(), global_slam_optimization_callback, 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 6e5e92a..886f0a2 100644 --- a/cartographer/mapping/map_builder.h +++ b/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; diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 3c29164..aa77203 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -58,7 +58,8 @@ class MapBuilderTest : public ::testing::Test { } void BuildMapBuilder() { - map_builder_ = common::make_unique(map_builder_options_); + map_builder_ = common::make_unique( + map_builder_options_, nullptr /* global_slam_optimization_callback */); } void SetOptionsTo3D() { diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 2c57826..5cfa18c 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -52,6 +52,10 @@ class PoseGraph : public PoseGraphInterface { common::Time time; }; + using GlobalSlamOptimizationCallback = + std::function&, + const std::map&)>; + PoseGraph() {} ~PoseGraph() override {}