diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index f72eaa6..c544388 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(j, submaps, matching_submap, insertion_submaps, + ComputeConstraintsForScan(j, matching_submap, insertion_submaps, finished_submap, pose, covariance); }); } @@ -140,6 +140,39 @@ void SparsePoseGraph::AddWorkItem(std::function work_item) { } } +void SparsePoseGraph::ComputeConstraint(const int scan_index, + const int submap_index) { + const transform::Rigid2d relative_pose = + submap_transforms_[submap_index].inverse() * + point_cloud_poses_[scan_index]; + + const mapping::Submaps* const scan_trajectory = + trajectory_nodes_[scan_index].constant_data->trajectory; + const mapping::Submaps* const submap_trajectory = + submap_states_[submap_index].trajectory; + // Only globally match against submaps not in this trajectory. + if (scan_trajectory != submap_trajectory && + global_localization_samplers_[scan_trajectory]->Pulse()) { + constraint_builder_.MaybeAddGlobalConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + scan_trajectory, submap_trajectory, &trajectory_connectivity_, + &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns); + } else { + const bool scan_and_submap_trajectories_connected = + reverse_connected_components_.count(scan_trajectory) > 0 && + reverse_connected_components_.count(submap_trajectory) > 0 && + reverse_connected_components_.at(scan_trajectory) == + reverse_connected_components_.at(submap_trajectory); + if (scan_trajectory == submap_trajectory || + scan_and_submap_trajectories_connected) { + constraint_builder_.MaybeAddConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns, + relative_pose); + } + } +} + void SparsePoseGraph::ComputeConstraintsForOldScans( const mapping::Submap* submap) { const int submap_index = GetSubmapIndex(submap); @@ -148,20 +181,13 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( const int num_nodes = point_cloud_poses_.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) { - const transform::Rigid2d relative_pose = - submap_transforms_[submap_index].inverse() * - point_cloud_poses_[scan_index]; - constraint_builder_.MaybeAddConstraint( - submap_index, submap, scan_index, - &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns, - relative_pose); + ComputeConstraint(scan_index, submap_index); } } } void SparsePoseGraph::ComputeConstraintsForScan( - int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submap* matching_submap, + int scan_index, const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) { @@ -194,42 +220,12 @@ void SparsePoseGraph::ComputeConstraintsForScan( Constraint::INTRA_SUBMAP}); } - // Determine if this scan should be globally localized. - const bool run_global_localization = - global_localization_samplers_[scan_trajectory]->Pulse(); - CHECK_LT(submap_states_.size(), std::numeric_limits::max()); const int num_submaps = submap_states_.size(); for (int submap_index = 0; submap_index != num_submaps; ++submap_index) { if (submap_states_[submap_index].finished) { CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0); - const transform::Rigid2d relative_pose = - submap_transforms_[submap_index].inverse() * - point_cloud_poses_[scan_index]; - - const auto* submap_trajectory = submap_states_[submap_index].trajectory; - - // Only globally match against submaps not in this trajectory. - if (run_global_localization && scan_trajectory != submap_trajectory) { - constraint_builder_.MaybeAddGlobalConstraint( - submap_index, submap_states_[submap_index].submap, scan_index, - scan_trajectory, submap_trajectory, &trajectory_connectivity_, - &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns); - } else { - const bool scan_and_submap_trajectories_connected = - reverse_connected_components_.count(scan_trajectory) > 0 && - reverse_connected_components_.count(submap_trajectory) > 0 && - reverse_connected_components_.at(scan_trajectory) == - reverse_connected_components_.at(submap_trajectory); - if (scan_trajectory == submap_trajectory || - scan_and_submap_trajectories_connected) { - constraint_builder_.MaybeAddConstraint( - submap_index, submap_states_[submap_index].submap, scan_index, - &trajectory_nodes_[scan_index] - .constant_data->laser_fan_2d.returns, - relative_pose); - } - } + ComputeConstraint(scan_index, submap_index); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 90ed4a4..e01ee31 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -127,12 +127,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( - int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submap* matching_submap, + int scan_index, const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_); + // Computes constraints for a scan and submap pair. + void ComputeConstraint(const int scan_index, const int submap_index) + REQUIRES(mutex_); + // Adds constraints for older scans whenever a new submap is finished. void ComputeConstraintsForOldScans(const mapping::Submap* submap) REQUIRES(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 6af6513..7bffa6c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -196,22 +196,24 @@ void ConstraintBuilder::ComputeConstraint( if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( filtered_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { + CHECK_GT(score, options_.global_localization_min_score()); trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); } else { return; } } else { - if (!submap_scan_matcher->fast_correlative_scan_matcher->Match( + if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, filtered_point_cloud, options_.min_score(), &score, &pose_estimate)) { + // We've reported a successful local match. + CHECK_GT(score, options_.min_score()); + } else { return; } - // We've reported a successful local match. - CHECK_GT(score, options_.min_score()); - { - common::MutexLocker locker(&mutex_); - score_histogram_.Add(score); - } + } + { + common::MutexLocker locker(&mutex_); + score_histogram_.Add(score); } // Use the CSM estimate as both the initial and previous pose. This has the diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 7b1ca57..a5e52e6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -122,9 +122,8 @@ int SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(time, j, submaps, matching_submap, - insertion_submaps, finished_submap, pose, - covariance); + ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps, + finished_submap, pose, covariance); }); return j; } @@ -147,6 +146,38 @@ void SparsePoseGraph::AddImuData(common::Time time, }); } +void SparsePoseGraph::ComputeConstraint(const int scan_index, + const int submap_index) { + const transform::Rigid3d relative_pose = + submap_transforms_[submap_index].inverse() * + optimization_problem_.node_data().at(scan_index).point_cloud_pose; + + const mapping::Submaps* const scan_trajectory = + trajectory_nodes_[scan_index].constant_data->trajectory; + const mapping::Submaps* const submap_trajectory = + submap_states_[submap_index].trajectory; + // Only globally match against submaps not in this trajectory. + if (scan_trajectory != submap_trajectory && + global_localization_samplers_[scan_trajectory]->Pulse()) { + constraint_builder_.MaybeAddGlobalConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + scan_trajectory, submap_trajectory, &trajectory_connectivity_, + trajectory_nodes_); + } else { + const bool scan_and_submap_trajectories_connected = + reverse_connected_components_.count(scan_trajectory) > 0 && + reverse_connected_components_.count(submap_trajectory) > 0 && + reverse_connected_components_.at(scan_trajectory) == + reverse_connected_components_.at(submap_trajectory); + if (scan_trajectory == submap_trajectory || + scan_and_submap_trajectories_connected) { + constraint_builder_.MaybeAddConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + trajectory_nodes_, relative_pose); + } + } +} + void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const int submap_index = GetSubmapIndex(submap); const auto& node_data = optimization_problem_.node_data(); @@ -155,20 +186,15 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const int num_nodes = node_data.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) { - const transform::Rigid3d relative_pose = - submap_transforms_[submap_index].inverse() * - node_data[scan_index].point_cloud_pose; - constraint_builder_.MaybeAddConstraint(submap_index, submap, scan_index, - trajectory_nodes_, relative_pose); + ComputeConstraint(scan_index, submap_index); } } } void SparsePoseGraph::ComputeConstraintsForScan( const common::Time time, const int scan_index, - const Submaps* scan_trajectory, const Submap* matching_submap, - std::vector insertion_submaps, const Submap* finished_submap, - const transform::Rigid3d& pose, + const Submap* matching_submap, std::vector insertion_submaps, + const Submap* finished_submap, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) { GrowSubmapTransformsAsNeeded(insertion_submaps); const int matching_index = GetSubmapIndex(matching_submap); @@ -194,39 +220,12 @@ void SparsePoseGraph::ComputeConstraintsForScan( Constraint::INTRA_SUBMAP}); } - // Determine if this scan should be globally localized. - const bool run_global_localization = - global_localization_samplers_[scan_trajectory]->Pulse(); - CHECK_LT(submap_states_.size(), std::numeric_limits::max()); const int num_submaps = submap_states_.size(); for (int submap_index = 0; submap_index != num_submaps; ++submap_index) { if (submap_states_[submap_index].finished) { CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0); - const transform::Rigid3d relative_pose = - submap_transforms_[submap_index].inverse() * optimized_pose; - - const auto* submap_trajectory = submap_states_[submap_index].trajectory; - - // Only globally match against submaps not in this trajectory. - if (run_global_localization && scan_trajectory != submap_trajectory) { - constraint_builder_.MaybeAddGlobalConstraint( - submap_index, submap_states_[submap_index].submap, scan_index, - scan_trajectory, submap_trajectory, &trajectory_connectivity_, - trajectory_nodes_); - } else { - const bool scan_and_submap_trajectories_connected = - reverse_connected_components_.count(scan_trajectory) > 0 && - reverse_connected_components_.count(submap_trajectory) > 0 && - reverse_connected_components_.at(scan_trajectory) == - reverse_connected_components_.at(submap_trajectory); - if (scan_trajectory == submap_trajectory || - scan_and_submap_trajectories_connected) { - constraint_builder_.MaybeAddConstraint( - submap_index, submap_states_[submap_index].submap, scan_index, - trajectory_nodes_, relative_pose); - } - } + ComputeConstraint(scan_index, submap_index); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 8383644..c30d7f3 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -129,12 +129,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( - common::Time time, int scan_index, const Submaps* scan_trajectory, - const Submap* matching_submap, + common::Time time, int scan_index, const Submap* matching_submap, std::vector insertion_submaps, const Submap* finished_submap, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_); + // Computes constraints for a scan and submap pair. + void ComputeConstraint(const int scan_index, const int submap_index) + REQUIRES(mutex_); + // Adds constraints for older scans whenever a new submap is finished. void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_); @@ -167,7 +170,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); // We globally localize a fraction of the scans from each trajectory. - std::unordered_map> + std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); // Number of scans added since last loop closure. diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 0f15101..3f4206c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -146,7 +146,8 @@ void ConstraintBuilder::WhenDone( const std::function callback) { common::MutexLocker locker(&mutex_); CHECK(when_done_ == nullptr); - when_done_.reset(new std::function(callback)); + when_done_ = + common::make_unique>(callback); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; thread_pool_->Schedule( @@ -223,13 +224,14 @@ void ConstraintBuilder::ComputeConstraint( CHECK(!match_full_submap) << "match_full_submap not supported for 3D."; - if (!submap_scan_matcher->fast_correlative_scan_matcher->Match( + if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, filtered_point_cloud, point_cloud, options_.min_score(), &score, &pose_estimate)) { + // We've reported a successful local match. + CHECK_GT(score, options_.min_score()); + } else { return; } - // We've reported a successful local match. - CHECK_GT(score, options_.min_score()); { common::MutexLocker locker(&mutex_); score_histogram_.Add(score);