From 5aad2d6feb2eb8fb254cd6ea51c66b20306059de Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 13 Oct 2016 17:52:05 +0200 Subject: [PATCH] Change 2D scan matching to use 3D point clouds. (#60) --- .../mapping_2d/local_trajectory_builder.cc | 5 +- .../scan_matching/ceres_scan_matcher.cc | 2 +- .../scan_matching/ceres_scan_matcher.h | 2 +- .../scan_matching/ceres_scan_matcher_test.cc | 4 +- .../scan_matching/correlative_scan_matcher.cc | 26 +++++----- .../scan_matching/correlative_scan_matcher.h | 8 +-- .../correlative_scan_matcher_test.cc | 24 ++++----- .../fast_correlative_scan_matcher.cc | 18 +++---- .../fast_correlative_scan_matcher.h | 6 +-- .../fast_correlative_scan_matcher_test.cc | 51 +++++++++++-------- .../scan_matching/fast_global_localizer.cc | 4 +- .../scan_matching/fast_global_localizer.h | 2 +- .../occupied_space_cost_functor.h | 4 +- .../real_time_correlative_scan_matcher.cc | 12 ++--- .../real_time_correlative_scan_matcher.h | 2 +- ...real_time_correlative_scan_matcher_test.cc | 24 +++++---- .../sparse_pose_graph/constraint_builder.cc | 4 +- 17 files changed, 104 insertions(+), 94 deletions(-) diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index c1c34da..6f10d11 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -107,9 +107,8 @@ void LocalTrajectoryBuilder::ScanMatch( transform::Rigid2d initial_ceres_pose = pose_prediction_2d; sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.adaptive_voxel_filter_options()); - const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d = - sensor::ProjectToPointCloud2D( - adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns)); + const sensor::PointCloud filtered_point_cloud_in_tracking_2d = + adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns); if (options_.use_online_correlative_scan_matching()) { real_time_correlative_scan_matcher_.Match( pose_prediction_2d, filtered_point_cloud_in_tracking_2d, diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index 23e6a3f..82f5c2c 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -66,7 +66,7 @@ CeresScanMatcher::~CeresScanMatcher() {} void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, + const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* const pose_estimate, kalman_filter::Pose2DCovariance* const covariance, diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h index 97e403f..3abd49f 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h @@ -49,7 +49,7 @@ class CeresScanMatcher { // the solver 'summary'. void Match(const transform::Rigid2d& previous_pose, const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, + const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, kalman_filter::Pose2DCovariance* covariance, diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index 2667dd0..a2098e7 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -40,7 +40,7 @@ class CeresScanMatcherTest : public ::testing::Test { probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), mapping::kMaxProbability); - point_cloud_.emplace_back(-3., 2.); + point_cloud_.emplace_back(-3.f, 2.f, 0.f); auto parameter_dictionary = common::MakeDictionary(R"text( return { @@ -74,7 +74,7 @@ class CeresScanMatcherTest : public ::testing::Test { } ProbabilityGrid probability_grid_; - sensor::PointCloud2D point_cloud_; + sensor::PointCloud point_cloud_; std::unique_ptr ceres_scan_matcher_; }; diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc index 809aced..c5b985a 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc @@ -26,14 +26,14 @@ namespace scan_matching { SearchParameters::SearchParameters(const double linear_search_window, const double angular_search_window, - const sensor::PointCloud2D& point_cloud, + const sensor::PointCloud& point_cloud, const double resolution) : resolution(resolution) { // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution; - for (const Eigen::Vector2f& point : point_cloud) { - const float range = point.norm(); + for (const Eigen::Vector3f& point : point_cloud) { + const float range = point.head<2>().norm(); max_scan_range = std::max(range, max_scan_range); } const double kSafetyMargin = 1. - 1e-3; @@ -91,10 +91,10 @@ void SearchParameters::ShrinkToFit(const std::vector& scans, } } -std::vector GenerateRotatedScans( - const sensor::PointCloud2D& point_cloud, +std::vector GenerateRotatedScans( + const sensor::PointCloud& point_cloud, const SearchParameters& search_parameters) { - std::vector rotated_scans; + std::vector rotated_scans; rotated_scans.reserve(search_parameters.num_scans); double delta_theta = -search_parameters.num_angular_perturbations * @@ -102,22 +102,24 @@ std::vector GenerateRotatedScans( for (int scan_index = 0; scan_index < search_parameters.num_scans; ++scan_index, delta_theta += search_parameters.angular_perturbation_step_size) { - rotated_scans.push_back(sensor::TransformPointCloud2D( - point_cloud, transform::Rigid2f::Rotation(delta_theta))); + rotated_scans.push_back(sensor::TransformPointCloud( + point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf( + delta_theta, Eigen::Vector3f::UnitZ())))); } return rotated_scans; } std::vector DiscretizeScans( - const MapLimits& map_limits, const std::vector& scans, + const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation) { std::vector discrete_scans; discrete_scans.reserve(scans.size()); - for (const sensor::PointCloud2D& scan : scans) { + for (const sensor::PointCloud& scan : scans) { discrete_scans.emplace_back(); discrete_scans.back().reserve(scan.size()); - for (const Eigen::Vector2f& point : scan) { - const Eigen::Vector2f translated_point = initial_translation * point; + for (const Eigen::Vector3f& point : scan) { + const Eigen::Vector2f translated_point = + Eigen::Affine2f(initial_translation) * point.head<2>(); discrete_scans.back().push_back( map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(), translated_point.y())); diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h index a07bcaa..2a36e46 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h @@ -42,7 +42,7 @@ struct SearchParameters { }; SearchParameters(double linear_search_window, double angular_search_window, - const sensor::PointCloud2D& point_cloud, double resolution); + const sensor::PointCloud& point_cloud, double resolution); // For testing. SearchParameters(int num_linear_perturbations, int num_angular_perturbations, @@ -60,14 +60,14 @@ struct SearchParameters { }; // Generates a collection of rotated scans. -std::vector GenerateRotatedScans( - const sensor::PointCloud2D& point_cloud, +std::vector GenerateRotatedScans( + const sensor::PointCloud& point_cloud, const SearchParameters& search_parameters); // Translates and discretizes the rotated scans into a vector of integer // indices. std::vector DiscretizeScans( - const MapLimits& map_limits, const std::vector& scans, + const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation); // A possible solution. diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc index a039045..93f44e0 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc @@ -57,9 +57,9 @@ TEST(Candidate, Construction) { } TEST(GenerateRotatedScans, GenerateRotatedScans) { - sensor::PointCloud2D point_cloud; - point_cloud.emplace_back(-1., 1.); - const std::vector scans = + sensor::PointCloud point_cloud; + point_cloud.emplace_back(-1.f, 1.f, 0.f); + const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.)); EXPECT_EQ(3, scans.size()); EXPECT_NEAR(1., scans[0][0].x(), 1e-6); @@ -71,17 +71,17 @@ TEST(GenerateRotatedScans, GenerateRotatedScans) { } TEST(DiscretizeScans, DiscretizeScans) { - sensor::PointCloud2D point_cloud; - point_cloud.emplace_back(0.025, 0.175); - point_cloud.emplace_back(-0.025, 0.175); - point_cloud.emplace_back(-0.075, 0.175); - point_cloud.emplace_back(-0.125, 0.175); - point_cloud.emplace_back(-0.125, 0.125); - point_cloud.emplace_back(-0.125, 0.075); - point_cloud.emplace_back(-0.125, 0.025); + sensor::PointCloud point_cloud; + point_cloud.emplace_back(0.025f, 0.175f, 0.f); + point_cloud.emplace_back(-0.025f, 0.175f, 0.f); + point_cloud.emplace_back(-0.075f, 0.175f, 0.f); + point_cloud.emplace_back(-0.125f, 0.175f, 0.f); + point_cloud.emplace_back(-0.125f, 0.125f, 0.f); + point_cloud.emplace_back(-0.125f, 0.075f, 0.f); + point_cloud.emplace_back(-0.125f, 0.025f, 0.f); const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)); - const std::vector scans = + const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity()); diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc index bf2dc5e..d73dc81 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc @@ -208,8 +208,8 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} bool FastCorrelativeScanMatcher::Match( const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, const float min_score, - float* score, transform::Rigid2d* pose_estimate) const { + const sensor::PointCloud& point_cloud, const float min_score, float* score, + transform::Rigid2d* pose_estimate) const { const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(), point_cloud, limits_.resolution()); @@ -219,7 +219,7 @@ bool FastCorrelativeScanMatcher::Match( } bool FastCorrelativeScanMatcher::MatchFullSubmap( - const sensor::PointCloud2D& point_cloud, float min_score, float* score, + const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const { // Compute a search window around the center of the submap that includes it // fully. @@ -239,17 +239,17 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( bool FastCorrelativeScanMatcher::MatchWithSearchParameters( SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, float min_score, float* score, + const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); - const sensor::PointCloud2D rotated_point_cloud = - sensor::TransformPointCloud2D( - point_cloud, - transform::Rigid2d::Rotation(initial_rotation).cast()); - const std::vector rotated_scans = + const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud( + point_cloud, + transform::Rigid3f::Rotation(Eigen::AngleAxisf( + initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); + const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); const std::vector discrete_scans = DiscretizeScans( limits_, rotated_scans, diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h index 5602e7e..0218233 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h @@ -111,14 +111,14 @@ class FastCorrelativeScanMatcher { // is possible, true is returned, and 'score' and 'pose_estimate' are updated // with the result. bool Match(const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, float min_score, + const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; // Aligns 'point_cloud' within the full 'probability_grid', i.e., not // restricted to the configured search window. If a score above 'min_score' // (excluding equality) is possible, true is returned, and 'score' and // 'pose_estimate' are updated with the result. - bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score, + bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; private: @@ -128,7 +128,7 @@ class FastCorrelativeScanMatcher { bool MatchWithSearchParameters( SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, float min_score, float* score, + const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; std::vector ComputeLowestResolutionCandidates( const std::vector& discrete_scans, diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index 5549140..eb51ffb 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -133,13 +133,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); - sensor::PointCloud2D point_cloud; - point_cloud.emplace_back(-2.5f, 0.5f); - point_cloud.emplace_back(-2.f, 0.5f); - point_cloud.emplace_back(0.f, -0.5f); - point_cloud.emplace_back(0.5f, -1.6f); - point_cloud.emplace_back(2.5f, 0.5f); - point_cloud.emplace_back(2.5f, 1.7f); + sensor::PointCloud point_cloud; + point_cloud.emplace_back(-2.5f, 0.5f, 0.f); + point_cloud.emplace_back(-2.f, 0.5f, 0.f); + point_cloud.emplace_back(0.f, -0.5f, 0.f); + point_cloud.emplace_back(0.5f, -1.6f, 0.f); + point_cloud.emplace_back(2.5f, 0.5f, 0.f); + point_cloud.emplace_back(2.5f, 1.7f, 0.f); for (int i = 0; i != 50; ++i) { const transform::Rigid2f expected_pose( @@ -149,11 +149,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); - laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(), - sensor::TransformPointCloud2D( - point_cloud, expected_pose), - {}}, - &probability_grid); + laser_fan_inserter.Insert( + sensor::LaserFan{ + expected_pose.translation(), + sensor::TransformPointCloud2D( + sensor::ProjectToPointCloud2D(point_cloud), expected_pose), + {}}, + &probability_grid); FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); @@ -177,20 +179,24 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); - sensor::PointCloud2D unperturbed_point_cloud; - unperturbed_point_cloud.emplace_back(-2.5, 0.5); - unperturbed_point_cloud.emplace_back(-2.25, 0.5); - unperturbed_point_cloud.emplace_back(0., 0.5); - unperturbed_point_cloud.emplace_back(0.25, 1.6); - unperturbed_point_cloud.emplace_back(2.5, 0.5); - unperturbed_point_cloud.emplace_back(2.0, 1.8); + sensor::PointCloud unperturbed_point_cloud; + unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f); + unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f); + unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f); + unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f); + unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f); + unperturbed_point_cloud.emplace_back(2.0f, 1.8f, 0.f); for (int i = 0; i != 20; ++i) { const transform::Rigid2f perturbation( {10. * distribution(prng), 10. * distribution(prng)}, 1.6 * distribution(prng)); - const sensor::PointCloud2D point_cloud = - sensor::TransformPointCloud2D(unperturbed_point_cloud, perturbation); + const sensor::PointCloud point_cloud = sensor::TransformPointCloud( + unperturbed_point_cloud, + transform::Rigid3f(Eigen::Vector3f(perturbation.translation().x(), + perturbation.translation().y(), 0.f), + Eigen::AngleAxisf(perturbation.rotation().angle(), + Eigen::Vector3f::UnitZ()))); const transform::Rigid2f expected_pose = transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)}, 0.5 * distribution(prng)) * @@ -202,7 +208,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { laser_fan_inserter.Insert( sensor::LaserFan{ (expected_pose * perturbation).translation(), - sensor::TransformPointCloud2D(point_cloud, expected_pose), + sensor::TransformPointCloud2D( + sensor::ProjectToPointCloud2D(point_cloud), expected_pose), {}}, &probability_grid); diff --git a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc index dbd6ba0..1399eeb 100644 --- a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc +++ b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc @@ -28,14 +28,14 @@ bool PerformGlobalLocalization( const std::vector< cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& matchers, - const cartographer::sensor::PointCloud2D& point_cloud, + const cartographer::sensor::PointCloud& point_cloud, transform::Rigid2d* const best_pose_estimate, float* const best_score) { CHECK(best_pose_estimate != nullptr) << "Need a non-null output_pose_estimate!"; CHECK(best_score != nullptr) << "Need a non-null best_score!"; *best_score = cutoff; transform::Rigid2d pose_estimate; - const sensor::PointCloud2D filtered_point_cloud = + const sensor::PointCloud filtered_point_cloud = voxel_filter.Filter(point_cloud); bool success = false; if (matchers.size() == 0) { diff --git a/cartographer/mapping_2d/scan_matching/fast_global_localizer.h b/cartographer/mapping_2d/scan_matching/fast_global_localizer.h index e7bee48..c9544d9 100644 --- a/cartographer/mapping_2d/scan_matching/fast_global_localizer.h +++ b/cartographer/mapping_2d/scan_matching/fast_global_localizer.h @@ -41,7 +41,7 @@ bool PerformGlobalLocalization( const std::vector< cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& matchers, - const cartographer::sensor::PointCloud2D& point_cloud, + const cartographer::sensor::PointCloud& point_cloud, transform::Rigid2d* best_pose_estimate, float* best_score); } // namespace scan_matching diff --git a/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h b/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h index 5932b0c..295ac5c 100644 --- a/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h +++ b/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h @@ -36,7 +36,7 @@ class OccupiedSpaceCostFunctor { // Creates an OccupiedSpaceCostFunctor using the specified map, resolution // level, and point cloud. OccupiedSpaceCostFunctor(const double scaling_factor, - const sensor::PointCloud2D& point_cloud, + const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), @@ -107,7 +107,7 @@ class OccupiedSpaceCostFunctor { }; const double scaling_factor_; - const sensor::PointCloud2D& point_cloud_; + const sensor::PointCloud& point_cloud_; const ProbabilityGrid& probability_grid_; }; diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc index 9dce002..38493a5 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc @@ -90,21 +90,21 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates( double RealTimeCorrelativeScanMatcher::Match( const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, + const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); - const sensor::PointCloud2D rotated_point_cloud = - sensor::TransformPointCloud2D( - point_cloud, - transform::Rigid2d::Rotation(initial_rotation).cast()); + const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud( + point_cloud, + transform::Rigid3f::Rotation(Eigen::AngleAxisf( + 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()); - const std::vector rotated_scans = + const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); const std::vector discrete_scans = DiscretizeScans( probability_grid.limits(), rotated_scans, diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h index 6dcc653..a28c4e7 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h @@ -68,7 +68,7 @@ class RealTimeCorrelativeScanMatcher { // 'initial_pose_estimate' then updates 'pose_estimate' with the result and // returns the score. double Match(const transform::Rigid2d& initial_pose_estimate, - const sensor::PointCloud2D& point_cloud, + const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const; diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index da765cb..fb039c2 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -49,16 +49,18 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { laser_fan_inserter_ = common::make_unique( CreateLaserFanInserterOptions(parameter_dictionary.get())); } - point_cloud_.emplace_back(0.025, 0.175); - point_cloud_.emplace_back(-0.025, 0.175); - point_cloud_.emplace_back(-0.075, 0.175); - point_cloud_.emplace_back(-0.125, 0.175); - point_cloud_.emplace_back(-0.125, 0.125); - point_cloud_.emplace_back(-0.125, 0.075); - point_cloud_.emplace_back(-0.125, 0.025); + point_cloud_.emplace_back(0.025f, 0.175f, 0.f); + point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); + point_cloud_.emplace_back(-0.075f, 0.175f, 0.f); + point_cloud_.emplace_back(-0.125f, 0.175f, 0.f); + point_cloud_.emplace_back(-0.125f, 0.125f, 0.f); + point_cloud_.emplace_back(-0.125f, 0.075f, 0.f); + point_cloud_.emplace_back(-0.125f, 0.025f, 0.f); probability_grid_.StartUpdate(); laser_fan_inserter_->Insert( - sensor::LaserFan{Eigen::Vector2f::Zero(), point_cloud_, {}}, + sensor::LaserFan{Eigen::Vector2f::Zero(), + sensor::ProjectToPointCloud2D(point_cloud_), + {}}, &probability_grid_); { auto parameter_dictionary = common::MakeDictionary( @@ -77,14 +79,14 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { ProbabilityGrid probability_grid_; std::unique_ptr laser_fan_inserter_; - sensor::PointCloud2D point_cloud_; + sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; }; TEST_F(RealTimeCorrelativeScanMatcherTest, ScorePerfectHighResolutionCandidate) { - const std::vector scans = + 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()); @@ -102,7 +104,7 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, TEST_F(RealTimeCorrelativeScanMatcherTest, ScorePartiallyCorrectHighResolutionCandidate) { - const std::vector scans = + 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()); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 24df77f..63f8bcf 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -181,8 +181,8 @@ void ConstraintBuilder::ComputeConstraint( ComputeSubmapPose(*submap) * initial_relative_pose; const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_index); - const sensor::PointCloud2D filtered_point_cloud = - adaptive_voxel_filter_.Filter(*point_cloud); + const sensor::PointCloud filtered_point_cloud = + sensor::ToPointCloud(adaptive_voxel_filter_.Filter(*point_cloud)); // The 'constraint_transform' (i <- j) is computed from: // - a 'filtered_point_cloud' in j,