diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index b8875c1..6197ba7 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -106,21 +106,23 @@ bool FastCorrelativeScanMatcher::Match( const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, - float* const score, transform::Rigid3d* const pose_estimate) const { + float* const score, transform::Rigid3d* const pose_estimate, + float* const rotational_score) const { const SearchParameters search_parameters{ common::RoundToInt(options_.linear_xy_search_window() / resolution_), common::RoundToInt(options_.linear_z_search_window() / resolution_), options_.angular_search_window()}; - return MatchWithSearchParameters(search_parameters, initial_pose_estimate, - coarse_point_cloud, fine_point_cloud, - min_score, score, pose_estimate); + return MatchWithSearchParameters( + search_parameters, initial_pose_estimate, coarse_point_cloud, + fine_point_cloud, min_score, score, pose_estimate, rotational_score); } bool FastCorrelativeScanMatcher::MatchFullSubmap( const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, - float* const score, transform::Rigid3d* const pose_estimate) const { + float* const score, transform::Rigid3d* const pose_estimate, + float* const rotational_score) const { const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(), gravity_alignment); float max_point_distance = 0.f; @@ -132,9 +134,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( common::RoundToInt(max_point_distance / resolution_ + 0.5f); const SearchParameters search_parameters{linear_window_size, linear_window_size, M_PI}; - return MatchWithSearchParameters(search_parameters, initial_pose_estimate, - coarse_point_cloud, fine_point_cloud, - min_score, score, pose_estimate); + return MatchWithSearchParameters( + search_parameters, initial_pose_estimate, coarse_point_cloud, + fine_point_cloud, min_score, score, pose_estimate, rotational_score); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( @@ -142,7 +144,8 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, - float* const score, transform::Rigid3d* const pose_estimate) const { + float* const score, transform::Rigid3d* const pose_estimate, + float* const rotational_score) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); @@ -158,12 +161,14 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { *score = best_candidate.score; + const auto& discrete_scan = discrete_scans[best_candidate.scan_index]; *pose_estimate = (transform::Rigid3f( resolution_ * best_candidate.offset.matrix().cast(), Eigen::Quaternionf::Identity()) * - discrete_scans[best_candidate.scan_index].pose) + discrete_scan.pose) .cast(); + *rotational_score = discrete_scan.rotational_score; return true; } return false; @@ -171,8 +176,8 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, - const sensor::PointCloud& point_cloud, - const transform::Rigid3f& pose) const { + const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose, + const float rotational_score) const { std::vector> cell_indices_per_depth; const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0); std::vector full_resolution_cell_indices; @@ -210,7 +215,7 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( low_resolution_cell_at_start - low_resolution_search_window_start); } } - return DiscreteScan{pose, cell_indices_per_depth}; + return DiscreteScan{pose, cell_indices_per_depth, rotational_score}; } std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( @@ -253,7 +258,7 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( transform::AngleAxisVectorToRotationQuaternion(angle_axis) * initial_pose.rotation()); result.push_back( - DiscretizeScan(search_parameters, coarse_point_cloud, pose)); + DiscretizeScan(search_parameters, coarse_point_cloud, pose, scores[i])); } return result; } diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h index 3eb21c1..674cf5c 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -46,6 +46,7 @@ struct DiscreteScan { transform::Rigid3f pose; // Contains a vector of discretized scans for each 'depth'. std::vector> cell_indices_per_depth; + float rotational_score; }; struct Candidate { @@ -81,24 +82,27 @@ class FastCorrelativeScanMatcher { // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) - // is possible, true is returned, and 'score' and 'pose_estimate' are updated - // with the result. 'fine_point_cloud' is used to compute the rotational scan - // matcher score. + // is possible, true is returned, and 'score', 'pose_estimate', and + // 'rotational_score' are updated with the result. 'fine_point_cloud' is used + // to compute the rotational scan matcher score. bool Match(const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, float min_score, - float* score, transform::Rigid3d* pose_estimate) const; + float* score, transform::Rigid3d* pose_estimate, + float* rotational_score) const; // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which // is expected to be approximately gravity aligned. If a score above - // 'min_score' (excluding equality) is possible, true is returned, and 'score' - // and 'pose_estimate' are updated with the result. 'fine_point_cloud' is used - // to compute the rotational scan matcher score. + // 'min_score' (excluding equality) is possible, true is returned, and + // 'score', 'pose_estimate', and 'rotational_score' are updated with the + // result. 'fine_point_cloud' is used to compute the rotational scan matcher + // score. bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, float min_score, float* score, - transform::Rigid3d* pose_estimate) const; + transform::Rigid3d* pose_estimate, + float* rotational_score) const; private: struct SearchParameters { @@ -112,10 +116,11 @@ class FastCorrelativeScanMatcher { const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, float min_score, float* score, - transform::Rigid3d* pose_estimate) const; + transform::Rigid3d* pose_estimate, float* rotational_score) const; DiscreteScan DiscretizeScan(const SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, - const transform::Rigid3f& pose) const; + const transform::Rigid3f& pose, + float rotational_score) const; std::vector GenerateDiscreteScans( const SearchParameters& search_parameters, const sensor::PointCloud& coarse_point_cloud, diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index 074cf6e..29daa22 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -98,12 +98,14 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {}, options); + float score = 0.f; transform::Rigid3d pose_estimate; - float score; + float rotational_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher.Match( transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore, - &score, &pose_estimate)); + &score, &pose_estimate, &rotational_score)); EXPECT_LT(kMinScore, score); + EXPECT_LT(0.09f, rotational_score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.05f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index f6767ab..2ab1aa1 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -181,8 +181,9 @@ void ConstraintBuilder::ComputeConstraint( // The 'constraint_transform' (submap i <- scan j) is computed from: // - a 'filtered_point_cloud' in scan j and // - the initial guess 'initial_pose' (submap i <- scan j). - float score = 0.; + float score = 0.f; transform::Rigid3d pose_estimate; + float rotational_score = 0.f; // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. @@ -191,7 +192,8 @@ void ConstraintBuilder::ComputeConstraint( if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( initial_pose.rotation(), filtered_point_cloud, point_cloud, - options_.global_localization_min_score(), &score, &pose_estimate)) { + options_.global_localization_min_score(), &score, &pose_estimate, + &rotational_score)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); @@ -203,7 +205,7 @@ void ConstraintBuilder::ComputeConstraint( } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, filtered_point_cloud, point_cloud, - options_.min_score(), &score, &pose_estimate)) { + options_.min_score(), &score, &pose_estimate, &rotational_score)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { @@ -213,6 +215,7 @@ void ConstraintBuilder::ComputeConstraint( { common::MutexLocker locker(&mutex_); score_histogram_.Add(score); + rotational_score_histogram_.Add(rotational_score); } // Use the CSM estimate as both the initial and previous pose. This has the @@ -282,6 +285,8 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { 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); } constraints_.clear(); callback = std::move(when_done_); diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index f038061..8689397 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -185,8 +185,9 @@ class ConstraintBuilder { const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; scan_matching::CeresScanMatcher ceres_scan_matcher_; - // Histogram of scan matcher scores. + // Histograms of scan matcher scores. common::Histogram score_histogram_ GUARDED_BY(mutex_); + common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_); }; } // namespace sparse_pose_graph