diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index dac8c5b..99361ed 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -60,14 +60,17 @@ ConstraintBuilder3D::ConstraintBuilder3D( common::ThreadPoolInterface* const thread_pool) : options_(options), thread_pool_(thread_pool), + finish_node_task_(common::make_unique()), + when_done_task_(common::make_unique()), sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder3D::~ConstraintBuilder3D() { common::MutexLocker locker(&mutex_); + CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); + CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; - CHECK_EQ(pending_computations_.size(), 0); - CHECK_EQ(submap_queued_work_items_.size(), 0); + CHECK_EQ(num_started_nodes_, num_finished_nodes_); CHECK(when_done_ == nullptr); } @@ -81,21 +84,28 @@ void ConstraintBuilder3D::MaybeAddConstraint( .norm() > options_.max_constraint_distance()) { return; } - if (sampler_.Pulse()) { - common::MutexLocker locker(&mutex_); - constraints_.emplace_back(); - kQueueLengthMetric->Set(constraints_.size()); - auto* const constraint = &constraints_.back(); - ++pending_computations_[current_computation_]; - const int current_computation = current_computation_; - ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ - constant_data, global_node_pose, global_submap_pose, - constraint); - FinishComputation(current_computation); - }); + if (!sampler_.Pulse()) return; + + common::MutexLocker locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "MaybeAddConstraint was called while WhenDone was scheduled."; } + constraints_.emplace_back(); + kQueueLengthMetric->Set(constraints_.size()); + auto* const constraint = &constraints_.back(); + const auto* scan_matcher = + DispatchScanMatcherConstruction(submap_id, submap_nodes, submap); + auto constraint_task = common::make_unique(); + constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ + constant_data, global_node_pose, global_submap_pose, + *scan_matcher, constraint); + }); + constraint_task->AddDependency(scan_matcher->creation_task_handle); + auto constraint_task_handle = + thread_pool_->Schedule(std::move(constraint_task)); + finish_node_task_->AddDependency(constraint_task_handle); } void ConstraintBuilder3D::MaybeAddGlobalConstraint( @@ -105,80 +115,82 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint( const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation) { common::MutexLocker locker(&mutex_); + if (when_done_) { + LOG(WARNING) + << "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; + } constraints_.emplace_back(); kQueueLengthMetric->Set(constraints_.size()); auto* const constraint = &constraints_.back(); - ++pending_computations_[current_computation_]; - const int current_computation = current_computation_; - ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { - ComputeConstraint( - submap_id, node_id, true, /* match_full_submap */ - constant_data, transform::Rigid3d::Rotation(global_node_rotation), - transform::Rigid3d::Rotation(global_submap_rotation), constraint); - FinishComputation(current_computation); - }); + const auto* scan_matcher = + DispatchScanMatcherConstruction(submap_id, submap_nodes, submap); + auto constraint_task = common::make_unique(); + constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ + constant_data, + transform::Rigid3d::Rotation(global_node_rotation), + transform::Rigid3d::Rotation(global_submap_rotation), + *scan_matcher, constraint); + }); + constraint_task->AddDependency(scan_matcher->creation_task_handle); + auto constraint_task_handle = + thread_pool_->Schedule(std::move(constraint_task)); + finish_node_task_->AddDependency(constraint_task_handle); } void ConstraintBuilder3D::NotifyEndOfNode() { common::MutexLocker locker(&mutex_); - ++current_computation_; + CHECK(finish_node_task_ != nullptr); + finish_node_task_->SetWorkItem([this] { + common::MutexLocker locker(&mutex_); + ++num_finished_nodes_; + }); + auto finish_node_task_handle = + thread_pool_->Schedule(std::move(finish_node_task_)); + finish_node_task_ = common::make_unique(); + when_done_task_->AddDependency(finish_node_task_handle); + ++num_started_nodes_; } void ConstraintBuilder3D::WhenDone( const std::function& callback) { common::MutexLocker locker(&mutex_); CHECK(when_done_ == nullptr); + // TODO(gaschler): Consider using just std::function, it can also be empty. when_done_ = common::make_unique>(callback); - ++pending_computations_[current_computation_]; - const int current_computation = current_computation_; - thread_pool_->Schedule( - [this, current_computation] { FinishComputation(current_computation); }); -} - -void ConstraintBuilder3D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const SubmapId& submap_id, const std::vector& submap_nodes, - const Submap3D* const submap, const std::function& work_item) { - if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != - nullptr) { - thread_pool_->Schedule(work_item); - } else { - submap_queued_work_items_[submap_id].push_back(work_item); - if (submap_queued_work_items_[submap_id].size() == 1) { - thread_pool_->Schedule([=]() { - ConstructSubmapScanMatcher(submap_id, submap_nodes, submap); - }); - } - } -} - -void ConstraintBuilder3D::ConstructSubmapScanMatcher( - const SubmapId& submap_id, const std::vector& submap_nodes, - const Submap3D* const submap) { - auto submap_scan_matcher = - common::make_unique( - submap->high_resolution_hybrid_grid(), - &submap->low_resolution_hybrid_grid(), submap_nodes, - options_.fast_correlative_scan_matcher_options_3d()); - common::MutexLocker locker(&mutex_); - submap_scan_matchers_[submap_id] = {&submap->high_resolution_hybrid_grid(), - &submap->low_resolution_hybrid_grid(), - std::move(submap_scan_matcher)}; - for (const std::function& work_item : - submap_queued_work_items_[submap_id]) { - thread_pool_->Schedule(work_item); - } - submap_queued_work_items_.erase(submap_id); + CHECK(when_done_task_ != nullptr); + when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); }); + thread_pool_->Schedule(std::move(when_done_task_)); + when_done_task_ = common::make_unique(); } const ConstraintBuilder3D::SubmapScanMatcher* -ConstraintBuilder3D::GetSubmapScanMatcher(const SubmapId& submap_id) { - common::MutexLocker locker(&mutex_); - const SubmapScanMatcher* submap_scan_matcher = - &submap_scan_matchers_[submap_id]; - CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr); - return submap_scan_matcher; +ConstraintBuilder3D::DispatchScanMatcherConstruction( + const SubmapId& submap_id, const std::vector& submap_nodes, + const Submap3D* submap) { + if (submap_scan_matchers_.count(submap_id) != 0) { + return &submap_scan_matchers_.at(submap_id); + } + auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + submap_scan_matcher.high_resolution_hybrid_grid = + &submap->high_resolution_hybrid_grid(); + submap_scan_matcher.low_resolution_hybrid_grid = + &submap->low_resolution_hybrid_grid(); + auto& scan_matcher_options = + options_.fast_correlative_scan_matcher_options_3d(); + auto scan_matcher_task = common::make_unique(); + scan_matcher_task->SetWorkItem( + [&submap_scan_matcher, &scan_matcher_options, submap_nodes]() { + submap_scan_matcher.fast_correlative_scan_matcher = + common::make_unique( + *submap_scan_matcher.high_resolution_hybrid_grid, + submap_scan_matcher.low_resolution_hybrid_grid, submap_nodes, + scan_matcher_options); + }); + submap_scan_matcher.creation_task_handle = + thread_pool_->Schedule(std::move(scan_matcher_task)); + return &submap_scan_matchers_.at(submap_id); } void ConstraintBuilder3D::ComputeConstraint( @@ -186,10 +198,8 @@ void ConstraintBuilder3D::ComputeConstraint( const TrajectoryNode::Data* const constant_data, const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose, + const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) { - const SubmapScanMatcher* const submap_scan_matcher = - GetSubmapScanMatcher(submap_id); - // The 'constraint_transform' (submap i <- node j) is computed from: // - a 'high_resolution_point_cloud' in node j and // - the initial guess 'initial_pose' (submap i <- node j). @@ -203,7 +213,7 @@ void ConstraintBuilder3D::ComputeConstraint( if (match_full_submap) { kGlobalConstraintsSearchedMetric->Increment(); match_result = - submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( + submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap( global_node_pose.rotation(), global_submap_pose.rotation(), *constant_data, options_.global_localization_min_score()); if (match_result != nullptr) { @@ -221,7 +231,7 @@ void ConstraintBuilder3D::ComputeConstraint( } } else { kConstraintsSearchedMetric->Increment(); - match_result = submap_scan_matcher->fast_correlative_scan_matcher->Match( + match_result = submap_scan_matcher.fast_correlative_scan_matcher->Match( global_node_pose, global_submap_pose, *constant_data, options_.min_score()); if (match_result != nullptr) { @@ -252,9 +262,9 @@ void ConstraintBuilder3D::ComputeConstraint( ceres_scan_matcher_.Match(match_result->pose_estimate.translation(), match_result->pose_estimate, {{&constant_data->high_resolution_point_cloud, - submap_scan_matcher->high_resolution_hybrid_grid}, + submap_scan_matcher.high_resolution_hybrid_grid}, {&constant_data->low_resolution_point_cloud, - submap_scan_matcher->low_resolution_hybrid_grid}}, + submap_scan_matcher.low_resolution_hybrid_grid}}, &constraint_transform, &unused_summary); constraint->reset(new Constraint{ @@ -287,54 +297,45 @@ void ConstraintBuilder3D::ComputeConstraint( } } -void ConstraintBuilder3D::FinishComputation(const int computation_index) { +void ConstraintBuilder3D::RunWhenDoneCallback() { Result result; std::unique_ptr> callback; { common::MutexLocker locker(&mutex_); - if (--pending_computations_[computation_index] == 0) { - pending_computations_.erase(computation_index); + CHECK(when_done_ != nullptr); + for (const std::unique_ptr& constraint : constraints_) { + if (constraint == nullptr) continue; + result.push_back(*constraint); } - if (pending_computations_.empty()) { - CHECK_EQ(submap_queued_work_items_.size(), 0); - if (when_done_ != nullptr) { - for (const std::unique_ptr& constraint : constraints_) { - if (constraint != nullptr) { - result.push_back(*constraint); - } - } - if (options_.log_matches()) { - LOG(INFO) << constraints_.size() << " computations resulted in " - << result.size() << " additional constraints."; - LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10); - LOG(INFO) << "Rotational score histogram:\n" - << rotational_score_histogram_.ToString(10); - LOG(INFO) << "Low resolution score histogram:\n" - << low_resolution_score_histogram_.ToString(10); - } - constraints_.clear(); - callback = std::move(when_done_); - when_done_.reset(); - } + if (options_.log_matches()) { + LOG(INFO) << constraints_.size() << " computations resulted in " + << result.size() << " additional constraints.\n" + << "Score histogram:\n" + << score_histogram_.ToString(10) << "\n" + << "Rotational score histogram:\n" + << rotational_score_histogram_.ToString(10) << "\n" + << "Low resolution score histogram:\n" + << low_resolution_score_histogram_.ToString(10); } + constraints_.clear(); + callback = std::move(when_done_); + when_done_.reset(); kQueueLengthMetric->Set(constraints_.size()); } - if (callback != nullptr) { - (*callback)(result); - } + (*callback)(result); } int ConstraintBuilder3D::GetNumFinishedNodes() { common::MutexLocker locker(&mutex_); - if (pending_computations_.empty()) { - return current_computation_; - } - return pending_computations_.begin()->first; + return num_finished_nodes_; } void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); - CHECK(pending_computations_.empty()); + if (when_done_) { + LOG(WARNING) + << "DeleteScanMatcher was called while WhenDone was scheduled."; + } submap_scan_matchers_.erase(submap_id); } diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/cartographer/mapping/internal/constraints/constraint_builder_3d.h index fd3c1d3..37262ed 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -30,6 +30,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" +#include "cartographer/common/task.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" @@ -48,10 +49,10 @@ namespace constraints { // Asynchronously computes constraints. // -// Intermingle an arbitrary number of calls to MaybeAddConstraint() or -// MaybeAddGlobalConstraint, then call WhenDone(). After all computations are -// done the 'callback' will be called with the result and another -// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. +// Intermingle an arbitrary number of calls to 'MaybeAddConstraint', +// 'MaybeAddGlobalConstraint', and 'NotifyEndOfNode', then call 'WhenDone' once. +// After all computations are done the 'callback' will be called with the result +// and another MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. // // This class is thread-safe. class ConstraintBuilder3D { @@ -101,7 +102,8 @@ class ConstraintBuilder3D { void NotifyEndOfNode(); // Registers the 'callback' to be called with the results, after all - // computations triggered by MaybeAddConstraint() have finished. + // computations triggered by 'MaybeAdd*Constraint' have finished. + // 'callback' is executed in the 'ThreadPool'. void WhenDone(const std::function& callback); // Returns the number of consecutive finished nodes. @@ -118,24 +120,15 @@ class ConstraintBuilder3D { const HybridGrid* low_resolution_hybrid_grid; std::unique_ptr fast_correlative_scan_matcher; + std::weak_ptr creation_task_handle; }; - // Either schedules the 'work_item', or if needed, schedules the scan matcher - // construction and queues the 'work_item'. - void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const SubmapId& submap_id, - const std::vector& submap_nodes, const Submap3D* submap, - const std::function& work_item) REQUIRES(mutex_); - - // Constructs the scan matcher for a 'submap', then schedules its work items. - void ConstructSubmapScanMatcher( + // The returned 'grid' and 'fast_correlative_scan_matcher' must only be + // accessed after 'creation_task_handle' has completed. + const SubmapScanMatcher* DispatchScanMatcherConstruction( const SubmapId& submap_id, const std::vector& submap_nodes, const Submap3D* submap) - EXCLUDES(mutex_); - - // Returns the scan matcher for a submap, which has to exist. - const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id) - EXCLUDES(mutex_); + REQUIRES(mutex_); // Runs in a background thread and does computations for an additional // constraint. @@ -145,12 +138,11 @@ class ConstraintBuilder3D { const TrajectoryNode::Data* const constant_data, const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose, + const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) EXCLUDES(mutex_); - // Decrements the 'pending_computations_' count. If all computations are done, - // runs the 'when_done_' callback and resets the state. - void FinishComputation(int computation_index) EXCLUDES(mutex_); + void RunWhenDoneCallback() EXCLUDES(mutex_); const proto::ConstraintBuilderOptions options_; common::ThreadPoolInterface* thread_pool_; @@ -160,28 +152,27 @@ class ConstraintBuilder3D { std::unique_ptr> when_done_ GUARDED_BY(mutex_); - // Index of the node in reaction to which computations are currently - // added. This is always the highest node index seen so far, even when older + // TODO(gaschler): Use atomics instead of mutex to access these counters. + // Number of the node in reaction to which computations are currently + // added. This is always the number of nodes seen so far, even when older // nodes are matched against a new submap. - int current_computation_ GUARDED_BY(mutex_) = 0; + int num_started_nodes_ GUARDED_BY(mutex_) = 0; - // For each added node, maps to the number of pending computations that were - // added for it. - std::map pending_computations_ GUARDED_BY(mutex_); + int num_finished_nodes_ GUARDED_BY(mutex_) = 0; + + std::unique_ptr finish_node_task_ GUARDED_BY(mutex_); + + std::unique_ptr when_done_task_ GUARDED_BY(mutex_); // Constraints currently being computed in the background. A deque is used to - // keep pointers valid when adding more entries. + // keep pointers valid when adding more entries. Constraint search results + // with below-threshold scores are also 'nullptr'. std::deque> constraints_ GUARDED_BY(mutex_); - // Map of already constructed scan matchers by 'submap_id'. + // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); - // Map by 'submap_id' of scan matchers under construction, and the work - // to do once construction is done. - std::map>> - submap_queued_work_items_ GUARDED_BY(mutex_); - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher3D ceres_scan_matcher_;