diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index 8c02c45..de67b6f 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -58,14 +58,17 @@ ConstraintBuilder2D::ConstraintBuilder2D( 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()) {} ConstraintBuilder2D::~ConstraintBuilder2D() { 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); } @@ -77,94 +80,101 @@ void ConstraintBuilder2D::MaybeAddConstraint( 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->grid(), [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, node_id, - false, /* match_full_submap */ - constant_data, initial_relative_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->grid()); + auto constraint_task = common::make_unique(); + constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ + constant_data, initial_relative_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 ConstraintBuilder2D::MaybeAddGlobalConstraint( const SubmapId& submap_id, const Submap2D* const submap, const NodeId& node_id, const TrajectoryNode::Data* const constant_data) { 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->grid(), [=]() EXCLUDES(mutex_) { - ComputeConstraint( - submap_id, submap, node_id, true, /* match_full_submap */ - constant_data, transform::Rigid2d::Identity(), constraint); - FinishComputation(current_computation); - }); + const auto* scan_matcher = + DispatchScanMatcherConstruction(submap_id, submap->grid()); + auto constraint_task = common::make_unique(); + constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ + constant_data, transform::Rigid2d::Identity(), + *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 ConstraintBuilder2D::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 ConstraintBuilder2D::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 ConstraintBuilder2D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - const SubmapId& submap_id, const Grid2D* const grid, - 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, grid); }); - } - } -} - -void ConstraintBuilder2D::ConstructSubmapScanMatcher(const SubmapId& submap_id, - const Grid2D* const grid) { - auto submap_scan_matcher = - common::make_unique( - *grid, options_.fast_correlative_scan_matcher_options()); - common::MutexLocker locker(&mutex_); - submap_scan_matchers_[submap_id] = {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 ConstraintBuilder2D::SubmapScanMatcher* -ConstraintBuilder2D::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; +ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id, + const Grid2D* const grid) { + 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.grid = grid; + auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options(); + auto scan_matcher_task = common::make_unique(); + scan_matcher_task->SetWorkItem( + [&submap_scan_matcher, &scan_matcher_options]() { + submap_scan_matcher.fast_correlative_scan_matcher = + common::make_unique( + *submap_scan_matcher.grid, 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 ConstraintBuilder2D::ComputeConstraint( @@ -172,11 +182,10 @@ void ConstraintBuilder2D::ComputeConstraint( const NodeId& node_id, bool match_full_submap, const TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose, + const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) { const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose; - const SubmapScanMatcher* const submap_scan_matcher = - GetSubmapScanMatcher(submap_id); // The 'constraint_transform' (submap i <- node j) is computed from: // - a 'filtered_gravity_aligned_point_cloud' in node j, @@ -192,7 +201,7 @@ void ConstraintBuilder2D::ComputeConstraint( // 3. Refine. if (match_full_submap) { kGlobalConstraintsSearchedMetric->Increment(); - if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( + if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap( constant_data->filtered_gravity_aligned_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); @@ -205,7 +214,7 @@ void ConstraintBuilder2D::ComputeConstraint( } } else { kConstraintsSearchedMetric->Increment(); - if (submap_scan_matcher->fast_correlative_scan_matcher->Match( + if (submap_scan_matcher.fast_correlative_scan_matcher->Match( initial_pose, constant_data->filtered_gravity_aligned_point_cloud, options_.min_score(), &score, &pose_estimate)) { // We've reported a successful local match. @@ -227,7 +236,7 @@ void ConstraintBuilder2D::ComputeConstraint( ceres::Solver::Summary unused_summary; ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate, constant_data->filtered_gravity_aligned_point_cloud, - *submap_scan_matcher->grid, &pose_estimate, + *submap_scan_matcher.grid, &pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform = @@ -258,50 +267,40 @@ void ConstraintBuilder2D::ComputeConstraint( } } -void ConstraintBuilder2D::FinishComputation(const int computation_index) { +void ConstraintBuilder2D::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); - } - 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."; + LOG(INFO) << "Score histogram:\n" << 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 ConstraintBuilder2D::GetNumFinishedNodes() { common::MutexLocker locker(&mutex_); - if (pending_computations_.empty()) { - return current_computation_; - } - return pending_computations_.begin()->first; + return num_finished_nodes_; } void ConstraintBuilder2D::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_2d.h b/cartographer/mapping/internal/constraints/constraint_builder_2d.h index b465154..5ac1e55 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -29,6 +29,7 @@ #include "cartographer/common/histogram.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/2d/submap_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" @@ -49,10 +50,10 @@ transform::Rigid2d ComputeSubmapPose(const Submap2D& submap); // 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 ConstraintBuilder2D { @@ -92,7 +93,8 @@ class ConstraintBuilder2D { 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. @@ -108,21 +110,13 @@ class ConstraintBuilder2D { const Grid2D* 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 Grid2D* grid, - const std::function& work_item) REQUIRES(mutex_); - - // Constructs the scan matcher for a 'submap', then schedules its work items. - void ConstructSubmapScanMatcher(const SubmapId& submap_id, const Grid2D* grid) - EXCLUDES(mutex_); - - // Returns the scan matcher for a submap, which has to exist. - const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id) - EXCLUDES(mutex_); + // 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 Grid2D* grid) REQUIRES(mutex_); // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'compressed_point_cloud' do not change @@ -131,12 +125,11 @@ class ConstraintBuilder2D { const NodeId& node_id, bool match_full_submap, const TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_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 constraints::proto::ConstraintBuilderOptions options_; common::ThreadPoolInterface* thread_pool_; @@ -146,28 +139,27 @@ class ConstraintBuilder2D { 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::CeresScanMatcher2D ceres_scan_matcher_;