From 4d11a226ffe9c85b65d0638b4a4d40e4995a356b Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 23 Aug 2017 17:48:49 +0200 Subject: [PATCH] Expose low resolution matching scores. (#470) This is needed in preparation of adding a low resolution score histogram. --- .../sparse_pose_graph/constraint_builder.cc | 2 -- .../proto/constraint_builder_options.proto | 5 --- cartographer/mapping_2d/sparse_pose_graph.cc | 26 ++++++++-------- .../sparse_pose_graph/optimization_problem.cc | 4 +-- .../mapping_2d/sparse_pose_graph_test.cc | 2 +- .../fast_correlative_scan_matcher.cc | 5 ++- .../fast_correlative_scan_matcher.h | 3 +- .../fast_correlative_scan_matcher_test.cc | 22 +++++++------ .../scan_matching/low_resolution_matcher.cc | 31 ++++++------------- .../scan_matching/low_resolution_matcher.h | 5 ++- ...ast_correlative_scan_matcher_options.proto | 4 +++ .../sparse_pose_graph/constraint_builder.cc | 2 +- configuration_files/sparse_pose_graph.lua | 2 +- docs/source/configuration.rst | 9 +++--- 14 files changed, 55 insertions(+), 67 deletions(-) diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index ea01c36..2a6d72e 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -36,8 +36,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( sensor::CreateAdaptiveVoxelFilterOptions( parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); options.set_min_score(parameter_dictionary->GetDouble("min_score")); - options.set_min_low_resolution_score( - parameter_dictionary->GetDouble("min_low_resolution_score")); options.set_global_localization_min_score( parameter_dictionary->GetDouble("global_localization_min_score")); options.set_loop_closure_translation_weight( diff --git a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto index c8fa448..8e293ad 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -23,7 +23,6 @@ import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.p import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; message ConstraintBuilderOptions { - // Next ID: 18 // A constraint will be added if the proportion of added constraints to // potential constraints drops below this number. optional double sampling_ratio = 1; @@ -40,10 +39,6 @@ message ConstraintBuilderOptions { // Low scores indicate that the scan and map do not look similar. optional double min_score = 4; - // Threshold for the score of the low resolution grid below which a match is - // not considered. Only used for 3D. - optional double min_low_resolution_score = 17; - // Threshold below which global localizations are not trusted. optional double global_localization_min_score = 5; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 7eae8b2..045f421 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -71,8 +71,7 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( } CHECK_EQ(2, insertion_submaps.size()); const mapping::SubmapId last_submap_id{ - trajectory_id, - submap_data.at(trajectory_id).rbegin()->first}; + trajectory_id, submap_data.at(trajectory_id).rbegin()->first}; if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' // and 'insertions_submaps.back()' is new. @@ -193,9 +192,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) - .at(node_id.node_index - - optimization_problem_.num_trimmed_nodes( - node_id.trajectory_id)) + .at(node_id.node_index - optimization_problem_.num_trimmed_nodes( + node_id.trajectory_id)) .point_cloud_pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, @@ -348,19 +346,21 @@ void SparsePoseGraph::WaitForAllComputations() { common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) - << 100. * (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / + << 100. * + (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / (num_trajectory_nodes_ - num_finished_scans_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; - constraint_builder_.WhenDone([this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - common::MutexLocker locker(&mutex_); - constraints_.insert(constraints_.end(), result.begin(), result.end()); - notification = true; - }); + constraint_builder_.WhenDone( + [this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 128e010..597828d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -154,8 +154,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, const int submap_index = index_submap_data.first; const SubmapData& submap_data = index_submap_data.second; - C_submaps[trajectory_id].emplace( - submap_index, FromPose(submap_data.pose)); + C_submaps[trajectory_id].emplace(submap_index, + FromPose(submap_data.pose)); problem.AddParameterBlock( C_submaps[trajectory_id].at(submap_index).data(), 3); if (first_submap || frozen) { diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 21aba98..fee364d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -75,7 +75,6 @@ class SparsePoseGraphTest : public ::testing::Test { max_range = 50., }, min_score = 0.5, - min_low_resolution_score = 0.5, global_localization_min_score = 0.6, loop_closure_translation_weight = 1., loop_closure_rotation_weight = 1., @@ -100,6 +99,7 @@ class SparsePoseGraphTest : public ::testing::Test { full_resolution_depth = 3, rotational_histogram_size = 30, min_rotational_score = 0.1, + min_low_resolution_score = 0.5, linear_xy_search_window = 4., linear_z_search_window = 4., angular_search_window = 0.1, 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 01bd896..0370c0d 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -45,6 +45,8 @@ CreateFastCorrelativeScanMatcherOptions( parameter_dictionary->GetInt("rotational_histogram_size")); options.set_min_rotational_score( parameter_dictionary->GetDouble("min_rotational_score")); + options.set_min_low_resolution_score( + parameter_dictionary->GetDouble("min_low_resolution_score")); options.set_linear_xy_search_window( parameter_dictionary->GetDouble("linear_xy_search_window")); options.set_linear_z_search_window( @@ -354,7 +356,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound( // not have better score. return best_high_resolution_candidate; } else if ((*search_parameters.matching_function)( - GetPoseFromCandidate(discrete_scans, candidate))) { + GetPoseFromCandidate(discrete_scans, candidate)) >= + options_.min_low_resolution_score()) { // We found the best candidate that passes the matching function. return candidate; } 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 98f5866..20a8258 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -68,7 +68,8 @@ struct Candidate { bool operator>(const Candidate& other) const { return score > other.score; } }; -using MatchingFunction = std::function; +// Used to compute scores between 0 and 1 how well the given pose matches. +using MatchingFunction = std::function; class FastCorrelativeScanMatcher { public: 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 4de869a..b5f3455 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 @@ -72,6 +72,7 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { ", " "rotational_histogram_size = 30, " "min_rotational_score = 0.1, " + "min_low_resolution_score = 0.5, " "linear_xy_search_window = 0.8, " "linear_z_search_window = 0.8, " "angular_search_window = 0.3, " @@ -109,12 +110,13 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { std::uniform_real_distribution distribution_ = std::uniform_real_distribution(-1.f, 1.f); RangeDataInserter range_data_inserter_; - static constexpr float kMinScore = 0.1f; const proto::FastCorrelativeScanMatcherOptions options_; sensor::PointCloud point_cloud_; }; -constexpr float FastCorrelativeScanMatcherTest::kMinScore; +constexpr float kMinScore = 0.1f; +constexpr float kPassingLowResolutionScore = 1.f; +constexpr float kFailingLowResolutionScore = 0.f; TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { for (int i = 0; i != 20; ++i) { @@ -128,8 +130,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { float rotational_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return true; }, &score, &pose_estimate, - &rotational_score)); + [](const transform::Rigid3f&) { return kPassingLowResolutionScore; }, + &score, &pose_estimate, &rotational_score)); EXPECT_LT(kMinScore, score); EXPECT_LT(0.09f, rotational_score); EXPECT_THAT(expected_pose, @@ -138,8 +140,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); EXPECT_FALSE(fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return false; }, &score, &pose_estimate, - &rotational_score)); + [](const transform::Rigid3f&) { return kFailingLowResolutionScore; }, + &score, &pose_estimate, &rotational_score)); } } @@ -154,8 +156,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { float rotational_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return true; }, &score, &pose_estimate, - &rotational_score)); + [](const transform::Rigid3f&) { return kPassingLowResolutionScore; }, + &score, &pose_estimate, &rotational_score)); EXPECT_LT(kMinScore, score); EXPECT_LT(0.09f, rotational_score); EXPECT_THAT(expected_pose, @@ -164,8 +166,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return false; }, &score, &pose_estimate, - &rotational_score)); + [](const transform::Rigid3f&) { return kFailingLowResolutionScore; }, + &score, &pose_estimate, &rotational_score)); } } // namespace diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc index 0b27a0d..30b3a91 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc @@ -20,29 +20,16 @@ namespace cartographer { namespace mapping_3d { namespace scan_matching { -namespace { - -// TODO(zhengj, whess): Interpolate the Grid to get better score. -float EvaluateLowResolutionScore(const HybridGrid& low_resolution_grid, - const sensor::PointCloud& points) { - float score = 0.f; - for (const auto& point : points) { - score += low_resolution_grid.GetProbability( - low_resolution_grid.GetCellIndex(point)); - } - return score / points.size(); -} - -} // namespace - -std::function CreateLowResolutionMatcher( - const HybridGrid* low_resolution_grid, const sensor::PointCloud* points, - const float min_low_resolution_score) { +std::function CreateLowResolutionMatcher( + const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { return [=](const transform::Rigid3f& pose) { - return EvaluateLowResolutionScore( - *low_resolution_grid, - sensor::TransformPointCloud(*points, pose)) >= - min_low_resolution_score; + float score = 0.f; + for (const auto& point : sensor::TransformPointCloud(*points, pose)) { + // TODO(zhengj, whess): Interpolate the Grid to get better score. + score += low_resolution_grid->GetProbability( + low_resolution_grid->GetCellIndex(point)); + } + return score / points->size(); }; } diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h index 339d427..c6a804c 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h @@ -27,9 +27,8 @@ namespace cartographer { namespace mapping_3d { namespace scan_matching { -std::function CreateLowResolutionMatcher( - const HybridGrid* low_resolution_grid, const sensor::PointCloud* points, - float min_low_resolution_score); +std::function CreateLowResolutionMatcher( + const HybridGrid* low_resolution_grid, const sensor::PointCloud* points); } // namespace scan_matching } // namespace mapping_3d diff --git a/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto index db49cff..76f3e3c 100644 --- a/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto +++ b/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto @@ -30,6 +30,10 @@ message FastCorrelativeScanMatcherOptions { // Minimum score for the rotational scan matcher. optional double min_rotational_score = 4; + // Threshold for the score of the low resolution grid below which a match is + // not considered. Only used for 3D. + optional double min_low_resolution_score = 9; + // Linear search window in the plane orthogonal to gravity in which the best // possible scan alignment will be found. optional double linear_xy_search_window = 5; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 3c6cfc0..9268ada 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -192,7 +192,7 @@ void ConstraintBuilder::ComputeConstraint( const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( submap_scan_matcher->low_resolution_hybrid_grid, - &low_resolution_point_cloud, options_.min_low_resolution_score()); + &low_resolution_point_cloud); // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 2bd466f..b8fffb9 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -23,7 +23,6 @@ SPARSE_POSE_GRAPH = { max_range = 50., }, min_score = 0.55, - min_low_resolution_score = 0.55, global_localization_min_score = 0.6, loop_closure_translation_weight = 1.1e4, loop_closure_rotation_weight = 1e5, @@ -48,6 +47,7 @@ SPARSE_POSE_GRAPH = { full_resolution_depth = 3, rotational_histogram_size = 120, min_rotational_score = 0.77, + min_low_resolution_score = 0.55, linear_xy_search_window = 5., linear_z_search_window = 1., angular_search_window = math.rad(15.), diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 93f3a1f..a6c677b 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -96,7 +96,6 @@ cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions ===================================================================== double sampling_ratio - Next ID: 18 A constraint will be added if the proportion of added constraints to potential constraints drops below this number. @@ -111,10 +110,6 @@ double min_score Threshold for the scan match score below which a match is not considered. Low scores indicate that the scan and map do not look similar. -double min_low_resolution_score - Threshold for the score of the low resolution grid below which a match is - not considered. Only used for 3D. - double global_localization_min_score Threshold below which global localizations are not trusted. @@ -445,6 +440,10 @@ int32 rotational_histogram_size double min_rotational_score Minimum score for the rotational scan matcher. +double min_low_resolution_score + Threshold for the score of the low resolution grid below which a match is + not considered. Only used for 3D. + double linear_xy_search_window Linear search window in the plane orthogonal to gravity in which the best possible scan alignment will be found.