From 6eaf0f344dd0d36a94a678eddb73d39557487b53 Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 9 Nov 2017 16:08:16 +0100 Subject: [PATCH] FastCorrelativeScanMatcher outputs unique_ptr. (#643) --- .../fast_correlative_scan_matcher.cc | 46 +++++------- .../fast_correlative_scan_matcher.h | 46 ++++++------ .../fast_correlative_scan_matcher_test.cc | 70 +++++++++---------- .../sparse_pose_graph/constraint_builder.cc | 38 +++++----- 4 files changed, 95 insertions(+), 105 deletions(-) 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 3f5d4fa..2ae4424 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -156,12 +156,12 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} -bool FastCorrelativeScanMatcher::Match( +std::unique_ptr +FastCorrelativeScanMatcher::Match( const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose, - const mapping::TrajectoryNode::Data& constant_data, const float min_score, - float* const score, transform::Rigid3d* const pose_estimate, - float* const rotational_score, float* const low_resolution_score) const { + const mapping::TrajectoryNode::Data& constant_data, + const float min_score) const { const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud); const SearchParameters search_parameters{ @@ -173,16 +173,15 @@ bool FastCorrelativeScanMatcher::Match( global_submap_pose.cast(), constant_data.high_resolution_point_cloud, constant_data.rotational_scan_matcher_histogram, - constant_data.gravity_alignment, min_score, score, pose_estimate, - rotational_score, low_resolution_score); + constant_data.gravity_alignment, min_score); } -bool FastCorrelativeScanMatcher::MatchFullSubmap( +std::unique_ptr +FastCorrelativeScanMatcher::MatchFullSubmap( const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation, - const mapping::TrajectoryNode::Data& constant_data, const float min_score, - float* const score, transform::Rigid3d* const pose_estimate, - float* const rotational_score, float* const low_resolution_score) const { + const mapping::TrajectoryNode::Data& constant_data, + const float min_score) const { float max_point_distance = 0.f; for (const Eigen::Vector3f& point : constant_data.high_resolution_point_cloud) { @@ -201,22 +200,17 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( transform::Rigid3f::Rotation(global_submap_rotation.cast()), constant_data.high_resolution_point_cloud, constant_data.rotational_scan_matcher_histogram, - constant_data.gravity_alignment, min_score, score, pose_estimate, - rotational_score, low_resolution_score); + constant_data.gravity_alignment, min_score); } -bool FastCorrelativeScanMatcher::MatchWithSearchParameters( +std::unique_ptr +FastCorrelativeScanMatcher::MatchWithSearchParameters( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const transform::Rigid3f& global_node_pose, const transform::Rigid3f& global_submap_pose, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, - const Eigen::Quaterniond& gravity_alignment, const float min_score, - float* const score, transform::Rigid3d* const pose_estimate, - float* const rotational_score, float* const low_resolution_score) const { - CHECK_NOTNULL(score); - CHECK_NOTNULL(pose_estimate); - + const Eigen::Quaterniond& gravity_alignment, const float min_score) const { const std::vector discrete_scans = GenerateDiscreteScans( search_parameters, point_cloud, rotational_scan_matcher_histogram, gravity_alignment, global_node_pose, global_submap_pose); @@ -228,15 +222,13 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( search_parameters, discrete_scans, lowest_resolution_candidates, precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { - *score = best_candidate.score; - *pose_estimate = - 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 common::make_unique(Result{ + best_candidate.score, + GetPoseFromCandidate(discrete_scans, best_candidate).cast(), + discrete_scans[best_candidate.scan_index].rotational_score, + best_candidate.low_resolution_score}); } - return false; + return nullptr; } DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( 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 8b5903f..071aec3 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -49,6 +49,13 @@ using MatchingFunction = std::function; class FastCorrelativeScanMatcher { public: + struct Result { + float score; + transform::Rigid3d pose_estimate; + float rotational_score; + float low_resolution_score; + }; + FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, const HybridGrid* low_resolution_hybrid_grid, @@ -61,28 +68,23 @@ class FastCorrelativeScanMatcher { delete; // Aligns the node with the given 'constant_data' within the 'hybrid_grid' - // given 'global_node_pose' and 'global_submap_pose'. If a score above - // 'min_score' (excluding equality) is possible, true is returned, and - // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score' - // are updated with the result. - bool Match(const transform::Rigid3d& global_node_pose, - const transform::Rigid3d& global_submap_pose, - const mapping::TrajectoryNode::Data& constant_data, - float min_score, float* score, transform::Rigid3d* pose_estimate, - float* rotational_score, float* low_resolution_score) const; + // given 'global_node_pose' and 'global_submap_pose'. 'Result' is only + // returned if a score above 'min_score' (excluding equality) is possible. + std::unique_ptr Match( + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, + const mapping::TrajectoryNode::Data& constant_data, + float min_score) const; // Aligns the node with the given 'constant_data' within the 'hybrid_grid' // given rotations which are expected to be approximately gravity aligned. - // If a score above 'min_score' (excluding equality) is possible, true is - // returned, and 'score', 'pose_estimate', 'rotational_score', and - // 'low_resolution_score' are updated with the result. - bool MatchFullSubmap(const Eigen::Quaterniond& global_node_rotation, - const Eigen::Quaterniond& global_submap_rotation, - const mapping::TrajectoryNode::Data& constant_data, - float min_score, float* score, - transform::Rigid3d* pose_estimate, - float* rotational_score, - float* low_resolution_score) const; + // 'Result' is only returned if a score above 'min_score' (excluding equality) + // is possible. + std::unique_ptr MatchFullSubmap( + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation, + const mapping::TrajectoryNode::Data& constant_data, + float min_score) const; private: struct SearchParameters { @@ -92,15 +94,13 @@ class FastCorrelativeScanMatcher { const MatchingFunction* const low_resolution_matcher; }; - bool MatchWithSearchParameters( + std::unique_ptr MatchWithSearchParameters( const SearchParameters& search_parameters, const transform::Rigid3f& global_node_pose, const transform::Rigid3f& global_submap_pose, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, - const Eigen::Quaterniond& gravity_alignment, float min_score, - float* score, transform::Rigid3d* pose_estimate, float* rotational_score, - float* low_resolution_score) const; + const Eigen::Quaterniond& gravity_alignment, float min_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 1a8a8a6..5e4e9ff 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 @@ -140,26 +140,25 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { std::unique_ptr fast_correlative_scan_matcher( GetFastCorrelativeScanMatcher(options_, expected_pose)); - 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(), transform::Rigid3d::Identity(), - CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate, - &rotational_score, &low_resolution_score)); - EXPECT_LT(kMinScore, score); - EXPECT_LT(0.09f, rotational_score); - EXPECT_LT(0.14f, low_resolution_score); + const std::unique_ptr result = + fast_correlative_scan_matcher->Match( + transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), + CreateConstantData(point_cloud_), kMinScore); + EXPECT_THAT(result, testing::NotNull()); + EXPECT_LT(kMinScore, result->score); + EXPECT_LT(0.09f, result->rotational_score); + EXPECT_LT(0.14f, result->low_resolution_score); EXPECT_THAT(expected_pose, - transform::IsNearly(pose_estimate.cast(), 0.05f)) - << "Actual: " << transform::ToProto(pose_estimate).DebugString() + transform::IsNearly(result->pose_estimate.cast(), 0.05f)) + << "Actual: " << transform::ToProto(result->pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); - EXPECT_FALSE(fast_correlative_scan_matcher->Match( - transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), - CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore, - &score, &pose_estimate, &rotational_score, &low_resolution_score)) - << low_resolution_score; + + const std::unique_ptr + low_resolution_result = fast_correlative_scan_matcher->Match( + transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), + CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore); + EXPECT_THAT(low_resolution_result, testing::IsNull()) + << low_resolution_result->low_resolution_score; } } @@ -169,26 +168,25 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { std::unique_ptr fast_correlative_scan_matcher( GetFastCorrelativeScanMatcher(options_, expected_pose)); - 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(), Eigen::Quaterniond::Identity(), - CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate, - &rotational_score, &low_resolution_score)); - EXPECT_LT(kMinScore, score); - EXPECT_LT(0.09f, rotational_score); - EXPECT_LT(0.14f, low_resolution_score); + const std::unique_ptr result = + fast_correlative_scan_matcher->MatchFullSubmap( + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), + CreateConstantData(point_cloud_), kMinScore); + EXPECT_THAT(result, testing::NotNull()); + EXPECT_LT(kMinScore, result->score); + EXPECT_LT(0.09f, result->rotational_score); + EXPECT_LT(0.14f, result->low_resolution_score); EXPECT_THAT(expected_pose, - transform::IsNearly(pose_estimate.cast(), 0.05f)) - << "Actual: " << transform::ToProto(pose_estimate).DebugString() + transform::IsNearly(result->pose_estimate.cast(), 0.05f)) + << "Actual: " << transform::ToProto(result->pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); - EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap( - Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), - CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore, - &score, &pose_estimate, &rotational_score, &low_resolution_score)) - << low_resolution_score; + + const std::unique_ptr + low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), + CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore); + EXPECT_THAT(low_resolution_result, testing::IsNull()) + << low_resolution_result->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 88beb7f..eea640f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -179,43 +179,41 @@ void ConstraintBuilder::ComputeConstraint( // The 'constraint_transform' (submap i <- scan j) is computed from: // - a 'high_resolution_point_cloud' in scan j and // - the initial guess 'initial_pose' (submap i <- scan j). - float score = 0.f; - transform::Rigid3d pose_estimate; - float rotational_score = 0.f; - float low_resolution_score = 0.f; - // TODO(gaschler): Match methods should return unique_ptr. + std::unique_ptr + match_result; // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. // 2. Prune if the score is too low. // 3. Refine. if (match_full_submap) { - if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( + match_result = + submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( global_node_pose.rotation(), global_submap_pose.rotation(), - *constant_data, options_.global_localization_min_score(), &score, - &pose_estimate, &rotational_score, &low_resolution_score)) { - CHECK_GT(score, options_.global_localization_min_score()); + *constant_data, options_.global_localization_min_score()); + if (match_result != nullptr) { + CHECK_GT(match_result->score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); } else { return; } } else { - if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - global_node_pose, global_submap_pose, *constant_data, - options_.min_score(), &score, &pose_estimate, &rotational_score, - &low_resolution_score)) { + match_result = submap_scan_matcher->fast_correlative_scan_matcher->Match( + global_node_pose, global_submap_pose, *constant_data, + options_.min_score()); + if (match_result != nullptr) { // We've reported a successful local match. - CHECK_GT(score, options_.min_score()); + CHECK_GT(match_result->score, options_.min_score()); } else { return; } } { common::MutexLocker locker(&mutex_); - score_histogram_.Add(score); - rotational_score_histogram_.Add(rotational_score); - low_resolution_score_histogram_.Add(low_resolution_score); + score_histogram_.Add(match_result->score); + rotational_score_histogram_.Add(match_result->rotational_score); + low_resolution_score_histogram_.Add(match_result->low_resolution_score); } // Use the CSM estimate as both the initial and previous pose. This has the @@ -223,7 +221,8 @@ void ConstraintBuilder::ComputeConstraint( // CSM estimate. ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; - ceres_scan_matcher_.Match(pose_estimate, pose_estimate, + ceres_scan_matcher_.Match(match_result->pose_estimate, + match_result->pose_estimate, {{&constant_data->high_resolution_point_cloud, submap_scan_matcher->high_resolution_hybrid_grid}, {&constant_data->low_resolution_point_cloud, @@ -252,7 +251,8 @@ void ConstraintBuilder::ComputeConstraint( << difference.translation().norm() << " rotation " << std::setprecision(3) << transform::GetAngle(difference); } - info << " with score " << std::setprecision(1) << 100. * score << "%."; + info << " with score " << std::setprecision(1) << 100. * match_result->score + << "%."; LOG(INFO) << info.str(); } }