From 58d94aaa6823f8183fb444d7f430786a9d3107e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 5 Jan 2018 10:43:56 +0100 Subject: [PATCH] Refactor calling optimization into DispatchOptimization. (#729) I noticed that @jihoonl opened the PR #726 which performs a similar thing. As discussed in googlecartographer/cartographer_ros#613 (@cschuet has already taken a look), I pulled this out of #481 (a really old PR whose merging has been postponed), which is an example where re-running optimization is triggered from elsewhere as well (besides from `ComputeConstraintsForNode`). This refactoring makes libcartographer friendlier for use cases such as that one. An important detail is that I have changed the condition in `WaitForAllComputations` to also check if the work queue is empty. If there are other things on the worker queue besides `ComputeConstraintsForNode`, currently we will wrongfully conclude that all computations are done. (This detail was merged in #754, so it's no longer in the diff of this PR). Also missing is the same thing for 3D. I can add that when we settle on this. Also, I suggest that `run_loop_closure` gets renamed to `run_optimization`. --- cartographer/mapping_2d/pose_graph.cc | 42 ++++++++++++++++---------- cartographer/mapping_2d/pose_graph.h | 3 ++ cartographer/mapping_3d/pose_graph.cc | 43 +++++++++++++++++---------- cartographer/mapping_3d/pose_graph.h | 3 ++ 4 files changed, 59 insertions(+), 32 deletions(-) diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 6b3a1c9..61a3222 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -269,18 +269,21 @@ void PoseGraph::ComputeConstraintsForNode( } constraint_builder_.NotifyEndOfNode(); ++num_nodes_since_last_loop_closure_; + CHECK(!run_loop_closure_); if (options_.optimize_every_n_nodes() > 0 && num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) { - CHECK(!run_loop_closure_); - run_loop_closure_ = true; - // If there is a 'work_queue_' already, some other thread will take care. - if (work_queue_ == nullptr) { - work_queue_ = common::make_unique>>(); - HandleWorkQueue(); - } + DispatchOptimization(); } } +void PoseGraph::DispatchOptimization() { + run_loop_closure_ = true; + // If there is a 'work_queue_' already, some other thread will take care. + if (work_queue_ == nullptr) { + work_queue_ = common::make_unique>>(); + HandleWorkQueue(); + } +} common::Time PoseGraph::GetLatestNodeTime( const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { common::Time time = trajectory_nodes_.at(node_id).constant_data->time; @@ -384,8 +387,8 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) { for (const auto& submap : submap_data_.trajectory(trajectory_id)) { submap_data_.at(submap.id).state = SubmapState::kFinished; } - // TODO(jihoonl): Refactor HandleWorkQueue() logic from - // ComputeConstraintsForNode and call from here + CHECK(!run_loop_closure_); + DispatchOptimization(); }); } @@ -505,14 +508,21 @@ void PoseGraph::AddTrimmer(std::unique_ptr trimmer) { } void PoseGraph::RunFinalOptimization() { + { + common::MutexLocker locker(&mutex_); + AddWorkItem([this]() REQUIRES(mutex_) { + optimization_problem_.SetMaxNumIterations( + options_.max_num_final_iterations()); + DispatchOptimization(); + }); + AddWorkItem([this]() REQUIRES(mutex_) { + optimization_problem_.SetMaxNumIterations( + options_.optimization_problem_options() + .ceres_solver_options() + .max_num_iterations()); + }); + } WaitForAllComputations(); - optimization_problem_.SetMaxNumIterations( - options_.max_num_final_iterations()); - RunOptimization(); - optimization_problem_.SetMaxNumIterations( - options_.optimization_problem_options() - .ceres_solver_options() - .max_num_iterations()); } void PoseGraph::RunOptimization() { diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index c8af7d0..93de584 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/cartographer/mapping_2d/pose_graph.h @@ -215,6 +215,9 @@ class PoseGraph : public mapping::PoseGraph { // Whether the optimization has to be run before more data is added. bool run_loop_closure_ GUARDED_BY(mutex_) = false; + // Schedules optimization (i.e. loop closure) to run. + void DispatchOptimization() REQUIRES(mutex_); + // Current optimization problem. pose_graph::OptimizationProblem optimization_problem_; pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index df10c3a..a45467f 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -286,15 +286,19 @@ void PoseGraph::ComputeConstraintsForNode( } constraint_builder_.NotifyEndOfNode(); ++num_nodes_since_last_loop_closure_; + CHECK(!run_loop_closure_); if (options_.optimize_every_n_nodes() > 0 && num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) { - CHECK(!run_loop_closure_); - run_loop_closure_ = true; - // If there is a 'work_queue_' already, some other thread will take care. - if (work_queue_ == nullptr) { - work_queue_ = common::make_unique>>(); - HandleWorkQueue(); - } + DispatchOptimization(); + } +} + +void PoseGraph::DispatchOptimization() { + run_loop_closure_ = true; + // If there is a 'work_queue_' already, some other thread will take care. + if (work_queue_ == nullptr) { + work_queue_ = common::make_unique>>(); + HandleWorkQueue(); } } @@ -401,8 +405,8 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) { for (const auto& submap : submap_data_.trajectory(trajectory_id)) { submap_data_.at(submap.id).state = SubmapState::kFinished; } - // TODO(jihoonl): Refactor HandleWorkQueue() logic from - // ComputeConstraintsForNode and call from here + CHECK(!run_loop_closure_); + DispatchOptimization(); }); } @@ -507,14 +511,21 @@ void PoseGraph::AddTrimmer(std::unique_ptr trimmer) { } void PoseGraph::RunFinalOptimization() { + { + common::MutexLocker locker(&mutex_); + AddWorkItem([this]() REQUIRES(mutex_) { + optimization_problem_.SetMaxNumIterations( + options_.max_num_final_iterations()); + DispatchOptimization(); + }); + AddWorkItem([this]() REQUIRES(mutex_) { + optimization_problem_.SetMaxNumIterations( + options_.optimization_problem_options() + .ceres_solver_options() + .max_num_iterations()); + }); + } WaitForAllComputations(); - optimization_problem_.SetMaxNumIterations( - options_.max_num_final_iterations()); - RunOptimization(); - optimization_problem_.SetMaxNumIterations( - options_.optimization_problem_options() - .ceres_solver_options() - .max_num_iterations()); } void PoseGraph::LogResidualHistograms() { diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index c68b725..143caf4 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/cartographer/mapping_3d/pose_graph.h @@ -219,6 +219,9 @@ class PoseGraph : public mapping::PoseGraph { // Whether the optimization has to be run before more data is added. bool run_loop_closure_ GUARDED_BY(mutex_) = false; + // Schedules optimization (i.e. loop closure) to run. + void DispatchOptimization() REQUIRES(mutex_); + // Current optimization problem. pose_graph::OptimizationProblem optimization_problem_; pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);