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 0370c0d..a33b3ce 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -91,6 +91,39 @@ class PrecomputationGridStack { std::vector precomputation_grids_; }; +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 { + Candidate(const int scan_index, const Eigen::Array3i& offset) + : scan_index(scan_index), offset(offset) {} + + static Candidate Unsuccessful() { + return Candidate(0, Eigen::Array3i::Zero()); + } + + // Index into the discrete scans vectors. + int scan_index; + + // Linear offset from the initial pose in cell indices. For lower resolution + // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth + // block of possibilities. + Eigen::Array3i offset; + + // Score, higher is better. + float score = -std::numeric_limits::infinity(); + + // Score of the low resolution matcher. + float low_resolution_score = 0.f; + + bool operator<(const Candidate& other) const { return score < other.score; } + bool operator>(const Candidate& other) const { return score > other.score; } +}; + FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, const std::vector& nodes, @@ -108,25 +141,26 @@ 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, - const MatchingFunction& matching_function, float* const score, - transform::Rigid3d* const pose_estimate, - float* const rotational_score) const { + const MatchingFunction& low_resolution_matcher, float* const score, + transform::Rigid3d* const pose_estimate, float* const rotational_score, + float* const low_resolution_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(), &matching_function}; - return MatchWithSearchParameters( - search_parameters, initial_pose_estimate, coarse_point_cloud, - fine_point_cloud, min_score, score, pose_estimate, rotational_score); + options_.angular_search_window(), &low_resolution_matcher}; + return MatchWithSearchParameters(search_parameters, initial_pose_estimate, + coarse_point_cloud, fine_point_cloud, + min_score, score, pose_estimate, + rotational_score, low_resolution_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, - const MatchingFunction& matching_function, float* const score, - transform::Rigid3d* const pose_estimate, - float* const rotational_score) const { + const MatchingFunction& low_resolution_matcher, float* const score, + transform::Rigid3d* const pose_estimate, float* const rotational_score, + float* const low_resolution_score) const { const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(), gravity_alignment); float max_point_distance = 0.f; @@ -137,10 +171,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( (width_in_voxels_ + 1) / 2 + common::RoundToInt(max_point_distance / resolution_ + 0.5f); const SearchParameters search_parameters{ - linear_window_size, linear_window_size, M_PI, &matching_function}; - return MatchWithSearchParameters( - search_parameters, initial_pose_estimate, coarse_point_cloud, - fine_point_cloud, min_score, score, pose_estimate, rotational_score); + linear_window_size, linear_window_size, M_PI, &low_resolution_matcher}; + return MatchWithSearchParameters(search_parameters, initial_pose_estimate, + coarse_point_cloud, fine_point_cloud, + min_score, score, pose_estimate, + rotational_score, low_resolution_score); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( @@ -149,7 +184,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, float* const score, transform::Rigid3d* const pose_estimate, - float* const rotational_score) const { + float* const rotational_score, float* const low_resolution_score) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); @@ -169,6 +204,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( GetPoseFromCandidate(discrete_scans, best_candidate).cast(); *rotational_score = discrete_scans[best_candidate.scan_index].rotational_score; + *low_resolution_score = best_candidate.low_resolution_score; return true; } return false; @@ -347,26 +383,30 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound( const std::vector& discrete_scans, const std::vector& candidates, const int candidate_depth, float min_score) const { - Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero()); - best_high_resolution_candidate.score = min_score; if (candidate_depth == 0) { for (const Candidate& candidate : candidates) { if (candidate.score <= min_score) { // Return if the candidate is bad because the following candidate will // not have better score. - return best_high_resolution_candidate; - } else if ((*search_parameters.matching_function)( - GetPoseFromCandidate(discrete_scans, candidate)) >= - options_.min_low_resolution_score()) { + return Candidate::Unsuccessful(); + } + const float low_resolution_score = + (*search_parameters.low_resolution_matcher)( + GetPoseFromCandidate(discrete_scans, candidate)); + if (low_resolution_score >= options_.min_low_resolution_score()) { // We found the best candidate that passes the matching function. - return candidate; + Candidate best_candidate = candidate; + best_candidate.low_resolution_score = low_resolution_score; + return best_candidate; } } // All candidates have good scores but none passes the matching function. - return best_high_resolution_candidate; + return Candidate::Unsuccessful(); } + Candidate best_high_resolution_candidate = Candidate::Unsuccessful(); + best_high_resolution_candidate.score = min_score; for (const Candidate& candidate : candidates) { if (candidate.score <= min_score) { break; 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 20a8258..1fcce5f 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -41,32 +41,8 @@ CreateFastCorrelativeScanMatcherOptions( common::LuaParameterDictionary* parameter_dictionary); class PrecomputationGridStack; - -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 { - Candidate(const int scan_index, const Eigen::Array3i& offset) - : scan_index(scan_index), offset(offset) {} - - // Index into the discrete scans vectors. - int scan_index; - - // Linear offset from the initial pose in cell indices. For lower resolution - // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth - // block of possibilities. - Eigen::Array3i offset; - - // Score, higher is better. - float score = 0.f; - - bool operator<(const Candidate& other) const { return score < other.score; } - bool operator>(const Candidate& other) const { return score > other.score; } -}; +struct DiscreteScan; +struct Candidate; // Used to compute scores between 0 and 1 how well the given pose matches. using MatchingFunction = std::function; @@ -85,35 +61,37 @@ 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', 'pose_estimate', and - // 'rotational_score' 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', + // 'rotational_score', and 'low_resolution_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, - const MatchingFunction& matching_function, float* score, - transform::Rigid3d* pose_estimate, float* rotational_score) const; + const MatchingFunction& low_resolution_matcher, float* score, + transform::Rigid3d* pose_estimate, float* rotational_score, + float* low_resolution_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', 'pose_estimate', and 'rotational_score' are updated with the - // result. 'fine_point_cloud' is used to compute the rotational scan matcher - // score. + // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_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, - const MatchingFunction& matching_function, float* score, - transform::Rigid3d* pose_estimate, - float* rotational_score) const; + const MatchingFunction& low_resolution_matcher, + float* score, transform::Rigid3d* pose_estimate, + float* rotational_score, + float* low_resolution_score) const; private: struct SearchParameters { const int linear_xy_window_size; // voxels const int linear_z_window_size; // voxels const double angular_search_window; // radians - const MatchingFunction* const matching_function; + const MatchingFunction* const low_resolution_matcher; }; bool MatchWithSearchParameters( @@ -121,7 +99,8 @@ 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, float* rotational_score) const; + transform::Rigid3d* pose_estimate, float* rotational_score, + float* low_resolution_score) const; DiscreteScan DiscretizeScan(const SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose, 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 b5f3455..421b3c9 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 @@ -128,12 +128,14 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { float score = 0.f; transform::Rigid3d pose_estimate; float rotational_score = 0.f; + float low_resolution_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore, [](const transform::Rigid3f&) { return kPassingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score)); + &score, &pose_estimate, &rotational_score, &low_resolution_score)); EXPECT_LT(kMinScore, score); EXPECT_LT(0.09f, rotational_score); + EXPECT_LT(0.99f, low_resolution_score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.05f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() @@ -141,7 +143,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { EXPECT_FALSE(fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore, [](const transform::Rigid3f&) { return kFailingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score)); + &score, &pose_estimate, &rotational_score, &low_resolution_score)); } } @@ -154,12 +156,14 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { float score = 0.f; transform::Rigid3d pose_estimate; float rotational_score = 0.f; + float low_resolution_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore, [](const transform::Rigid3f&) { return kPassingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score)); + &score, &pose_estimate, &rotational_score, &low_resolution_score)); EXPECT_LT(kMinScore, score); EXPECT_LT(0.09f, rotational_score); + EXPECT_LT(0.99f, low_resolution_score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.05f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() @@ -167,7 +171,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore, [](const transform::Rigid3f&) { return kFailingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score)); + &score, &pose_estimate, &rotational_score, &low_resolution_score)); } } // namespace diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 9268ada..32a70c7 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -189,6 +189,7 @@ void ConstraintBuilder::ComputeConstraint( float score = 0.f; transform::Rigid3d pose_estimate; float rotational_score = 0.f; + float low_resolution_score = 0.f; const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( submap_scan_matcher->low_resolution_hybrid_grid, @@ -202,7 +203,7 @@ void ConstraintBuilder::ComputeConstraint( if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( initial_pose.rotation(), high_resolution_point_cloud, point_cloud, options_.global_localization_min_score(), low_resolution_matcher, - &score, &pose_estimate, &rotational_score)) { + &score, &pose_estimate, &rotational_score, &low_resolution_score)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); @@ -215,7 +216,7 @@ void ConstraintBuilder::ComputeConstraint( if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, high_resolution_point_cloud, point_cloud, options_.min_score(), low_resolution_matcher, &score, - &pose_estimate, &rotational_score)) { + &pose_estimate, &rotational_score, &low_resolution_score)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { @@ -226,6 +227,7 @@ void ConstraintBuilder::ComputeConstraint( common::MutexLocker locker(&mutex_); score_histogram_.Add(score); rotational_score_histogram_.Add(rotational_score); + low_resolution_score_histogram_.Add(low_resolution_score); } // Use the CSM estimate as both the initial and previous pose. This has the @@ -288,6 +290,8 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { 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_); diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 590810b..03e914a 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -190,6 +190,7 @@ class ConstraintBuilder { // Histograms of scan matcher scores. common::Histogram score_histogram_ GUARDED_BY(mutex_); common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_); + common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_); }; } // namespace sparse_pose_graph