diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index b25488f..08823e0 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -75,12 +75,9 @@ std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( transform::Rigid2d initial_ceres_pose = pose_prediction; if (options_.use_online_correlative_scan_matching()) { - CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(), - proto::GridOptions2D_GridType_PROBABILITY_GRID); const double score = real_time_correlative_scan_matcher_.Match( pose_prediction, filtered_gravity_aligned_point_cloud, - *static_cast(matching_submap->grid()), - &initial_ceres_pose); + *matching_submap->grid(), &initial_ceres_pose); kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); } diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index e0691b6..3590d31 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -71,22 +71,23 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, initial_pose_estimate.rotation().angle()}; ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); - if (grid.GetGridType() == GridType::PROBABILITY_GRID) { - problem.AddResidualBlock( - CreateOccupiedSpaceCostFunction2D( - options_.occupied_space_weight() / - std::sqrt(static_cast(point_cloud.size())), - point_cloud, grid), - nullptr /* loss function */, ceres_pose_estimate); - } else if (grid.GetGridType() == GridType::TSDF) { - problem.AddResidualBlock( - CreateTSDFMatchCostFunction2D( - options_.occupied_space_weight() / - std::sqrt(static_cast(point_cloud.size())), - point_cloud, static_cast(grid)), - nullptr /* loss function */, ceres_pose_estimate); - } else { - LOG(FATAL) << "Unknown GridType."; + switch (grid.GetGridType()) { + case GridType::PROBABILITY_GRID: + problem.AddResidualBlock( + CreateOccupiedSpaceCostFunction2D( + options_.occupied_space_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, grid), + nullptr /* loss function */, ceres_pose_estimate); + break; + case GridType::TSDF: + problem.AddResidualBlock( + CreateTSDFMatchCostFunction2D( + options_.occupied_space_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, static_cast(grid)), + nullptr /* loss function */, ceres_pose_estimate); + break; } CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc index 9d914f6..2464268 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -25,6 +25,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/tsdf_2d.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -32,6 +33,48 @@ namespace cartographer { namespace mapping { namespace scan_matching { +namespace { + +float ComputeCandidateScore(const TSDF2D& tsdf, + const DiscreteScan2D& discrete_scan, + int x_index_offset, int y_index_offset) { + float candidate_score = 0.f; + float summed_weight = 0.f; + for (const Eigen::Array2i& xy_index : discrete_scan) { + const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset, + xy_index.y() + y_index_offset); + const std::pair tsd_and_weight = + tsdf.GetTSDAndWeight(proposed_xy_index); + const float normalized_tsd_score = + (tsdf.GetMaxCorrespondenceCost() - std::abs(tsd_and_weight.first)) / + tsdf.GetMaxCorrespondenceCost(); + const float weight = tsd_and_weight.second; + candidate_score += normalized_tsd_score * weight; + summed_weight += weight; + } + if (summed_weight == 0.f) return 0.f; + candidate_score /= summed_weight; + CHECK_GE(candidate_score, 0.f); + return candidate_score; +} + +float ComputeCandidateScore(const ProbabilityGrid& probability_grid, + const DiscreteScan2D& discrete_scan, + int x_index_offset, int y_index_offset) { + float candidate_score = 0.f; + for (const Eigen::Array2i& xy_index : discrete_scan) { + const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset, + xy_index.y() + y_index_offset); + const float probability = + probability_grid.GetProbability(proposed_xy_index); + candidate_score += probability; + } + candidate_score /= static_cast(discrete_scan.size()); + CHECK_GT(candidate_score, 0.f); + return candidate_score; +} + +} // namespace RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D( const proto::RealTimeCorrelativeScanMatcherOptions& options) @@ -73,8 +116,7 @@ RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates( double RealTimeCorrelativeScanMatcher2D::Match( const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const sensor::PointCloud& point_cloud, const Grid2D& grid, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); @@ -85,18 +127,17 @@ double RealTimeCorrelativeScanMatcher2D::Match( initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); const SearchParameters search_parameters( options_.linear_search_window(), options_.angular_search_window(), - rotated_point_cloud, probability_grid.limits().resolution()); + rotated_point_cloud, grid.limits().resolution()); const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); const std::vector discrete_scans = DiscretizeScans( - probability_grid.limits(), rotated_scans, + grid.limits(), rotated_scans, Eigen::Translation2f(initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y())); std::vector candidates = GenerateExhaustiveSearchCandidates(search_parameters); - ScoreCandidates(probability_grid, discrete_scans, search_parameters, - &candidates); + ScoreCandidates(grid, discrete_scans, search_parameters, &candidates); const Candidate2D& best_candidate = *std::max_element(candidates.begin(), candidates.end()); @@ -108,29 +149,29 @@ double RealTimeCorrelativeScanMatcher2D::Match( } void RealTimeCorrelativeScanMatcher2D::ScoreCandidates( - const ProbabilityGrid& probability_grid, - const std::vector& discrete_scans, + const Grid2D& grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* const candidates) const { for (Candidate2D& candidate : *candidates) { - candidate.score = 0.f; - for (const Eigen::Array2i& xy_index : - discrete_scans[candidate.scan_index]) { - const Eigen::Array2i proposed_xy_index( - xy_index.x() + candidate.x_index_offset, - xy_index.y() + candidate.y_index_offset); - const float probability = - probability_grid.GetProbability(proposed_xy_index); - candidate.score += probability; + switch (grid.GetGridType()) { + case GridType::PROBABILITY_GRID: + candidate.score = ComputeCandidateScore( + static_cast(grid), + discrete_scans[candidate.scan_index], candidate.x_index_offset, + candidate.y_index_offset); + break; + case GridType::TSDF: + candidate.score = ComputeCandidateScore( + static_cast(grid), + discrete_scans[candidate.scan_index], candidate.x_index_offset, + candidate.y_index_offset); + break; } - candidate.score /= - static_cast(discrete_scans[candidate.scan_index].size()); candidate.score *= std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) * options_.translation_delta_cost_weight() + std::abs(candidate.orientation) * options_.rotation_delta_cost_weight())); - CHECK_GT(candidate.score, 0.f); } } diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h index ab68471..32789ad 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h @@ -41,7 +41,7 @@ #include #include "Eigen/Core" -#include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" #include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" @@ -60,20 +60,19 @@ class RealTimeCorrelativeScanMatcher2D { RealTimeCorrelativeScanMatcher2D& operator=( const RealTimeCorrelativeScanMatcher2D&) = delete; - // Aligns 'point_cloud' within the 'probability_grid' given an + // Aligns 'point_cloud' within the 'grid' given an // 'initial_pose_estimate' then updates 'pose_estimate' with the result and // returns the score. double Match(const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud& point_cloud, - const ProbabilityGrid& probability_grid, + const sensor::PointCloud& point_cloud, const Grid2D& grid, transform::Rigid2d* pose_estimate) const; // Computes the score for each Candidate2D in a collection. The cost is - // computed as the sum of probabilities, different from the Ceres - // CostFunctions: http://ceres-solver.org/modeling.html + // computed as the sum of probabilities or normalized TSD values, different + // from the Ceres CostFunctions: http://ceres-solver.org/modeling.html // // Visible for testing. - void ScoreCandidates(const ProbabilityGrid& probability_grid, + void ScoreCandidates(const Grid2D& grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* candidates) const; diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index 0e70341..9561843 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -24,6 +24,8 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" @@ -34,12 +36,66 @@ namespace mapping { namespace scan_matching { namespace { +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherTestOptions2D() { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "linear_search_window = 0.6, " + "angular_search_window = 0.16, " + "translation_delta_cost_weight = 0., " + "rotation_delta_cost_weight = 0., " + "}"); + return CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary.get()); +} + class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { protected: - RealTimeCorrelativeScanMatcherTest() - : probability_grid_( - MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)), - &conversion_tables_) { + RealTimeCorrelativeScanMatcherTest() { + point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}}); + point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}}); + real_time_correlative_scan_matcher_ = + absl::make_unique( + CreateRealTimeCorrelativeScanMatcherTestOptions2D()); + } + + void SetUpTSDF() { + grid_ = absl::make_unique( + MapLimits(0.05, Eigen::Vector2d(0.3, 0.5), CellLimits(20, 20)), 0.3, + 1.0, &conversion_tables_); + { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + truncation_distance = 0.3, + maximum_weight = 10., + update_free_space = false, + normal_estimation_options = { + num_normal_samples = 4, + sample_radius = 0.5, + }, + project_sdf_distance_to_scan_normal = true, + update_weight_range_exponent = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0.5, + update_weight_distance_cell_to_hit_kernel_bandwith = 0.5, + })text"); + range_data_inserter_ = absl::make_unique( + CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get())); + } + range_data_inserter_->Insert( + sensor::RangeData{Eigen::Vector3f(0.5f, -0.5f, 0.f), point_cloud_, {}}, + grid_.get()); + grid_->FinishUpdate(); + } + + void SetUpProbabilityGrid() { + grid_ = absl::make_unique( + MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)), + &conversion_tables_); { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -52,51 +108,31 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { CreateProbabilityGridRangeDataInserterOptions2D( parameter_dictionary.get())); } - point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}}); - point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}}); - point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}}); - point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}}); - point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}}); - point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}}); - point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}}); range_data_inserter_->Insert( sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, - &probability_grid_); - probability_grid_.FinishUpdate(); - { - auto parameter_dictionary = common::MakeDictionary( - "return {" - "linear_search_window = 0.6, " - "angular_search_window = 0.16, " - "translation_delta_cost_weight = 0., " - "rotation_delta_cost_weight = 0., " - "}"); - real_time_correlative_scan_matcher_ = - absl::make_unique( - CreateRealTimeCorrelativeScanMatcherOptions( - parameter_dictionary.get())); - } + grid_.get()); + grid_->FinishUpdate(); } ValueConversionTables conversion_tables_; - ProbabilityGrid probability_grid_; - std::unique_ptr range_data_inserter_; + std::unique_ptr grid_; + std::unique_ptr range_data_inserter_; sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; }; TEST_F(RealTimeCorrelativeScanMatcherTest, - ScorePerfectHighResolutionCandidate) { + ScorePerfectHighResolutionCandidateProbabilityGrid) { + SetUpProbabilityGrid(); const std::vector scans = GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); - const std::vector discrete_scans = DiscretizeScans( - probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); std::vector candidates; candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); real_time_correlative_scan_matcher_->ScoreCandidates( - probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), - &candidates); + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); EXPECT_EQ(0, candidates[0].scan_index); EXPECT_EQ(0, candidates[0].x_index_offset); EXPECT_EQ(0, candidates[0].y_index_offset); @@ -105,16 +141,35 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, } TEST_F(RealTimeCorrelativeScanMatcherTest, - ScorePartiallyCorrectHighResolutionCandidate) { + ScorePerfectHighResolutionCandidateTSDF) { + SetUpTSDF(); const std::vector scans = GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); - const std::vector discrete_scans = DiscretizeScans( - probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(0, candidates[0].y_index_offset); + // Every point should align perfectly. + EXPECT_NEAR(1.0, candidates[0].score, 1e-1); + EXPECT_LT(0.95, candidates[0].score); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePartiallyCorrectHighResolutionCandidateProbabilityGrid) { + SetUpProbabilityGrid(); + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); std::vector candidates; candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); real_time_correlative_scan_matcher_->ScoreCandidates( - probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), - &candidates); + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); EXPECT_EQ(0, candidates[0].scan_index); EXPECT_EQ(0, candidates[0].x_index_offset); EXPECT_EQ(1, candidates[0].y_index_offset); @@ -123,6 +178,25 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, EXPECT_GT(0.7, candidates[0].score); } +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePartiallyCorrectHighResolutionCandidateTSDF) { + SetUpTSDF(); + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(1, candidates[0].y_index_offset); + // 3 points should align perfectly. + EXPECT_LT(1.0 - 4. / (7. * 6.), candidates[0].score); + EXPECT_GT(1.0, candidates[0].score); +} + } // namespace } // namespace scan_matching } // namespace mapping