From a5dafcfde36f48f43891e91de96c13f62c3de684 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 7 Sep 2017 14:51:19 +0200 Subject: [PATCH] Update trajectory connection in WhenDone callback. (#506) This modifies the ConstraintBuilder interface so that Maybe*Constraint is not responsible anymore for updating the TrajectoryConnectivity member of SparsePoseGraph and moves this responsibility into the WhenDone callback. --- cartographer/mapping_2d/sparse_pose_graph.cc | 12 ++++++++++-- cartographer/mapping_2d/sparse_pose_graph.h | 2 +- .../sparse_pose_graph/constraint_builder.cc | 9 ++------- .../sparse_pose_graph/constraint_builder.h | 12 ++---------- cartographer/mapping_3d/sparse_pose_graph.cc | 11 ++++++++++- cartographer/mapping_3d/sparse_pose_graph.h | 2 +- .../sparse_pose_graph/constraint_builder.cc | 9 ++------- .../sparse_pose_graph/constraint_builder.h | 11 ++--------- 8 files changed, 30 insertions(+), 38 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 02f0c3f..72e9479 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -178,8 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, global_localization_samplers_[node_id.trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get(), - &trajectory_connectivity_); + trajectory_nodes_.at(node_id).constant_data.get()); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(node_id.trajectory_id) > 0 && @@ -313,6 +312,15 @@ void SparsePoseGraph::HandleWorkQueue() { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); } + + // Update the trajectory connectivity structure with the new + // constraints. + for (const Constraint& constraint : result) { + CHECK_EQ(constraint.tag, + mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); + trajectory_connectivity_.Connect(constraint.node_id.trajectory_id, + constraint.submap_id.trajectory_id); + } RunOptimization(); common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 1e927b7..ffb2dd2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -172,7 +172,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GUARDED_BY(mutex_); // How our various trajectories are related. - mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); + mapping::TrajectoryConnectivity trajectory_connectivity_; // We globally localize a fraction of the scans from each trajectory. std::unordered_map> diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 2a89761..33a5131 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -77,7 +77,6 @@ void ConstraintBuilder::MaybeAddConstraint( submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ - nullptr, /* trajectory_connectivity */ constant_data, initial_relative_pose, constraint); FinishComputation(current_computation); }); @@ -87,8 +86,7 @@ void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - mapping::TrajectoryConnectivity* const trajectory_connectivity) { + const mapping::TrajectoryNode::Data* const constant_data) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); @@ -98,7 +96,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ - trajectory_connectivity, constant_data, + constant_data, transform::Rigid2d::Identity(), constraint); FinishComputation(current_computation); }); @@ -162,7 +160,6 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, bool match_full_submap, - mapping::TrajectoryConnectivity* trajectory_connectivity, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose, std::unique_ptr* constraint) { @@ -190,8 +187,6 @@ void ConstraintBuilder::ComputeConstraint( CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); - trajectory_connectivity->Connect(node_id.trajectory_id, - submap_id.trajectory_id); } else { return; } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index e9cb33f..1505929 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -32,7 +32,6 @@ #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" -#include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submaps.h" @@ -87,15 +86,12 @@ class ConstraintBuilder { // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. // This performs full-submap matching. // - // The 'trajectory_connectivity' is updated if the full-submap match succeeds. - // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, - const mapping::TrajectoryNode::Data* const constant_data, - mapping::TrajectoryConnectivity* trajectory_connectivity); + const mapping::TrajectoryNode::Data* const constant_data); // Must be called after all computations related to one node have been added. void NotifyEndOfScan(); @@ -134,14 +130,10 @@ class ConstraintBuilder { // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'compressed_point_cloud' do not change - // anymore. If 'match_full_submap' is true, and global localization succeeds, - // will connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in - // 'trajectory_connectivity'. - // As output, it may create a new Constraint in 'constraint'. + // anymore. As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, bool match_full_submap, - mapping::TrajectoryConnectivity* trajectory_connectivity, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid2d& initial_relative_pose, std::unique_ptr* constraint) EXCLUDES(mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 9cef64f..8da99a8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -210,7 +210,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - initial_relative_pose.rotation(), &trajectory_connectivity_); + initial_relative_pose.rotation()); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(node_id.trajectory_id) > 0 && @@ -328,6 +328,15 @@ void SparsePoseGraph::HandleWorkQueue() { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); } + + // Update the trajectory connectivity structure with the new + // constraints. + for (const Constraint& constraint : result) { + CHECK_EQ(constraint.tag, + mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); + trajectory_connectivity_.Connect(constraint.node_id.trajectory_id, + constraint.submap_id.trajectory_id); + } RunOptimization(); common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 42d5a83..6cfdd16 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -177,7 +177,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GUARDED_BY(mutex_); // How our various trajectories are related. - mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); + mapping::TrajectoryConnectivity trajectory_connectivity_; // We globally localize a fraction of the scans from each trajectory. std::unordered_map> diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 9502ad1..a589e06 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -72,7 +72,6 @@ void ConstraintBuilder::MaybeAddConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ - nullptr, /* trajectory_connectivity */ constant_data, initial_pose, constraint); FinishComputation(current_computation); }); @@ -84,8 +83,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, - const Eigen::Quaterniond& gravity_alignment, - mapping::TrajectoryConnectivity* const trajectory_connectivity) { + const Eigen::Quaterniond& gravity_alignment) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); @@ -94,7 +92,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ - trajectory_connectivity, constant_data, + constant_data, transform::Rigid3d::Rotation(gravity_alignment), constraint); FinishComputation(current_computation); @@ -167,7 +165,6 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, bool match_full_submap, - mapping::TrajectoryConnectivity* trajectory_connectivity, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid3d& initial_pose, std::unique_ptr* constraint) { @@ -194,8 +191,6 @@ void ConstraintBuilder::ComputeConstraint( CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); - trajectory_connectivity->Connect(node_id.trajectory_id, - submap_id.trajectory_id); } else { return; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 4693f37..d3161dc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -31,7 +31,6 @@ #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" @@ -85,8 +84,7 @@ class ConstraintBuilder { // This performs full-submap matching. // // The 'gravity_alignment' is the rotation to apply to the point cloud data - // to make it approximately gravity aligned. The 'trajectory_connectivity' is - // updated if the full-submap match succeeds. + // to make it approximately gravity aligned. // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. @@ -95,8 +93,7 @@ class ConstraintBuilder { const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, - const Eigen::Quaterniond& gravity_alignment, - mapping::TrajectoryConnectivity* trajectory_connectivity); + const Eigen::Quaterniond& gravity_alignment); // Must be called after all computations related to one node have been added. void NotifyEndOfScan(); @@ -136,14 +133,10 @@ class ConstraintBuilder { // Runs in a background thread and does computations for an additional // constraint. - // If 'match_full_submap' is true, and global localization succeeds, will - // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in - // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, bool match_full_submap, - mapping::TrajectoryConnectivity* trajectory_connectivity, const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid3d& initial_pose, std::unique_ptr* constraint) EXCLUDES(mutex_);