diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index 2a6d72e..ea01c36 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -36,6 +36,8 @@ 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 8e293ad..c8fa448 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -23,6 +23,7 @@ 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; @@ -39,6 +40,10 @@ 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_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index a718258..21aba98 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -75,6 +75,7 @@ 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., 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 6197ba7..01bd896 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -106,12 +106,13 @@ 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 MatchingFunction& matching_function, 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()}; + 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); @@ -121,7 +122,8 @@ 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 MatchingFunction& matching_function, float* const score, + transform::Rigid3d* const pose_estimate, float* const rotational_score) const { const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(), gravity_alignment); @@ -132,8 +134,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( const int linear_window_size = (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}; + 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); @@ -161,14 +163,10 @@ 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_scan.pose) - .cast(); - *rotational_score = discrete_scan.rotational_score; + GetPoseFromCandidate(discrete_scans, best_candidate).cast(); + *rotational_score = + discrete_scans[best_candidate.scan_index].rotational_score; return true; } return false; @@ -334,18 +332,38 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( return lowest_resolution_candidates; } +transform::Rigid3f FastCorrelativeScanMatcher::GetPoseFromCandidate( + const std::vector& discrete_scans, + const Candidate& candidate) const { + return transform::Rigid3f::Translation( + resolution_ * candidate.offset.matrix().cast()) * + discrete_scans[candidate.scan_index].pose; +} + Candidate FastCorrelativeScanMatcher::BranchAndBound( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const std::vector& discrete_scans, const std::vector& candidates, const int candidate_depth, float min_score) const { - if (candidate_depth == 0) { - // Return the best candidate. - return *candidates.begin(); - } - 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))) { + // We found the best candidate that passes the matching function. + return candidate; + } + } + + // All candidates have good scores but none passes the matching function. + return best_high_resolution_candidate; + } + 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 674cf5c..98f5866 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -68,6 +68,8 @@ struct Candidate { bool operator>(const Candidate& other) const { return score > other.score; } }; +using MatchingFunction = std::function; + class FastCorrelativeScanMatcher { public: FastCorrelativeScanMatcher( @@ -88,8 +90,8 @@ class FastCorrelativeScanMatcher { 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, - float* rotational_score) const; + const MatchingFunction& matching_function, 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 @@ -100,7 +102,8 @@ class FastCorrelativeScanMatcher { bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, - float min_score, float* score, + float min_score, + const MatchingFunction& matching_function, float* score, transform::Rigid3d* pose_estimate, float* rotational_score) const; @@ -109,6 +112,7 @@ class FastCorrelativeScanMatcher { 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; }; bool MatchWithSearchParameters( @@ -138,6 +142,9 @@ class FastCorrelativeScanMatcher { const std::vector& discrete_scans, const std::vector& candidates, int candidate_depth, float min_score) const; + transform::Rigid3f GetPoseFromCandidate( + const std::vector& discrete_scans, + const Candidate& candidate) const; const proto::FastCorrelativeScanMatcherOptions options_; const float resolution_; 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 29daa22..4de869a 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 @@ -22,6 +22,7 @@ #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/make_unique.h" #include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" @@ -32,87 +33,141 @@ namespace mapping_3d { namespace scan_matching { namespace { -proto::FastCorrelativeScanMatcherOptions -CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { - auto parameter_dictionary = common::MakeDictionary( - "return {" - "branch_and_bound_depth = " + - std::to_string(branch_and_bound_depth) + - ", " - "full_resolution_depth = " + - std::to_string(branch_and_bound_depth) + - ", " - "rotational_histogram_size = 30, " - "min_rotational_score = 0.1, " - "linear_xy_search_window = 0.8, " - "linear_z_search_window = 0.8, " - "angular_search_window = 0.3, " - "}"); - return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); -} +class FastCorrelativeScanMatcherTest : public ::testing::Test { + protected: + FastCorrelativeScanMatcherTest() + : range_data_inserter_(CreateRangeDataInserterTestOptions()), + options_(CreateFastCorrelativeScanMatcherTestOptions(5)){}; -mapping_3d::proto::RangeDataInserterOptions -CreateRangeDataInserterTestOptions() { - auto parameter_dictionary = common::MakeDictionary( - "return { " - "hit_probability = 0.7, " - "miss_probability = 0.4, " - "num_free_space_voxels = 5, " - "}"); - return CreateRangeDataInserterOptions(parameter_dictionary.get()); -} + void SetUp() override { + point_cloud_ = { + Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f), + Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f), + Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f), + Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f), + Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f), + Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)}; + } -TEST(FastCorrelativeScanMatcherTest, CorrectPose) { - std::mt19937 prng(42); - std::uniform_real_distribution distribution(-1.f, 1.f); - RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); - constexpr float kMinScore = 0.1f; - const auto options = CreateFastCorrelativeScanMatcherTestOptions(5); + transform::Rigid3f GetRandomPose() { + const float x = 0.7f * distribution_(prng_); + const float y = 0.7f * distribution_(prng_); + const float z = 0.7f * distribution_(prng_); + const float theta = 0.2f * distribution_(prng_); + return transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) * + transform::Rigid3f::Rotation( + Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); + } - sensor::PointCloud point_cloud{ - Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f), - Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f), - Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f), - Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f), - Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f), - Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)}; + static proto::FastCorrelativeScanMatcherOptions + CreateFastCorrelativeScanMatcherTestOptions( + const int branch_and_bound_depth) { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "branch_and_bound_depth = " + + std::to_string(branch_and_bound_depth) + + ", " + "full_resolution_depth = " + + std::to_string(branch_and_bound_depth) + + ", " + "rotational_histogram_size = 30, " + "min_rotational_score = 0.1, " + "linear_xy_search_window = 0.8, " + "linear_z_search_window = 0.8, " + "angular_search_window = 0.3, " + "}"); + return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); + } - for (int i = 0; i != 20; ++i) { - const float x = 0.7f * distribution(prng); - const float y = 0.7f * distribution(prng); - const float z = 0.7f * distribution(prng); - const float theta = 0.2f * distribution(prng); - const auto expected_pose = - transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) * - transform::Rigid3f::Rotation( - Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); + static mapping_3d::proto::RangeDataInserterOptions + CreateRangeDataInserterTestOptions() { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "num_free_space_voxels = 5, " + "}"); + return CreateRangeDataInserterOptions(parameter_dictionary.get()); + } + std::unique_ptr GetFastCorrelativeScanMatcher( + const proto::FastCorrelativeScanMatcherOptions& options, + const transform::Rigid3f& pose) { HybridGrid hybrid_grid(0.05f); - range_data_inserter.Insert( - sensor::RangeData{ - expected_pose.translation(), - sensor::TransformPointCloud(point_cloud, expected_pose), - {}}, + range_data_inserter_.Insert( + sensor::RangeData{pose.translation(), + sensor::TransformPointCloud(point_cloud_, pose), + {}}, &hybrid_grid); hybrid_grid.FinishUpdate(); - FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {}, - options); + return common::make_unique( + hybrid_grid, std::vector(), options); + } + + std::mt19937 prng_ = std::mt19937(42); + 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; + +TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { + for (int i = 0; i != 20; ++i) { + const auto expected_pose = GetRandomPose(); + + std::unique_ptr fast_correlative_scan_matcher( + GetFastCorrelativeScanMatcher(options_, expected_pose)); + float score = 0.f; transform::Rigid3d pose_estimate; float rotational_score = 0.f; - EXPECT_TRUE(fast_correlative_scan_matcher.Match( - transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore, - &score, &pose_estimate, &rotational_score)); + 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)); 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() << "\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)); } } +TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { + const auto expected_pose = GetRandomPose(); + + std::unique_ptr fast_correlative_scan_matcher( + GetFastCorrelativeScanMatcher(options_, expected_pose)); + + float score = 0.f; + transform::Rigid3d pose_estimate; + 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)); + 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() + << "\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)); +} + } // namespace } // namespace scan_matching } // namespace mapping_3d diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc new file mode 100644 index 0000000..0b27a0d --- /dev/null +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc @@ -0,0 +1,51 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h" + +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) { + return [=](const transform::Rigid3f& pose) { + return EvaluateLowResolutionScore( + *low_resolution_grid, + sensor::TransformPointCloud(*points, pose)) >= + min_low_resolution_score; + }; +} + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h new file mode 100644 index 0000000..339d427 --- /dev/null +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.h @@ -0,0 +1,38 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ + +#include + +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" + +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); + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_ diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index d007c0b..3c6cfc0 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -29,6 +29,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" @@ -179,6 +180,9 @@ void ConstraintBuilder::ComputeConstraint( const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress(); const sensor::PointCloud high_resolution_point_cloud = high_resolution_adaptive_voxel_filter_.Filter(point_cloud); + const sensor::PointCloud low_resolution_point_cloud = + low_resolution_adaptive_voxel_filter_.Filter(point_cloud); + // 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). @@ -186,6 +190,10 @@ void ConstraintBuilder::ComputeConstraint( transform::Rigid3d pose_estimate; float rotational_score = 0.f; + const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( + submap_scan_matcher->low_resolution_hybrid_grid, + &low_resolution_point_cloud, options_.min_low_resolution_score()); + // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. // 2. Prune if the score is too low. @@ -193,8 +201,8 @@ void ConstraintBuilder::ComputeConstraint( if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( initial_pose.rotation(), high_resolution_point_cloud, point_cloud, - options_.global_localization_min_score(), &score, &pose_estimate, - &rotational_score)) { + options_.global_localization_min_score(), low_resolution_matcher, + &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); @@ -206,7 +214,8 @@ void ConstraintBuilder::ComputeConstraint( } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, high_resolution_point_cloud, point_cloud, - options_.min_score(), &score, &pose_estimate, &rotational_score)) { + options_.min_score(), low_resolution_matcher, &score, + &pose_estimate, &rotational_score)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { @@ -222,9 +231,6 @@ void ConstraintBuilder::ComputeConstraint( // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original // CSM estimate. - const sensor::PointCloud low_resolution_point_cloud = - low_resolution_adaptive_voxel_filter_.Filter(point_cloud); - ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; ceres_scan_matcher_.Match(pose_estimate, pose_estimate, diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 69697c0..2bd466f 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -23,6 +23,7 @@ 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, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index a9c997d..93f3a1f 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -96,6 +96,7 @@ 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. @@ -110,6 +111,10 @@ 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.