From a42fd47146c667266d9b1cfa2afe5cefd5634c16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Mon, 23 Jul 2018 19:02:17 +0200 Subject: [PATCH] Preparation for #1277: refactor rotation scan matcher (#1316) Added new constructor for `RotationalScanMatcher` and exposed `RotateHistogram`. This PR prepares for PR #1277 where the constructor `RotationalScanMatcher(const std::vector>& histograms_at_angles)` will be removed and only the new constructor `RotationalScanMatcher(const Eigen::VectorXf& histogram)` will remain. The unit tests will be updated to use the new constructor in #1277. --- .../scan_matching/rotational_scan_matcher.cc | 39 ++++++++++--------- .../scan_matching/rotational_scan_matcher.h | 8 ++++ .../rotational_scan_matcher_test.cc | 9 +++-- 3 files changed, 34 insertions(+), 22 deletions(-) diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc index 2e2e92f..3722aae 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -117,10 +117,23 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) { return result; } -// Rotates the given 'histogram' by the given 'angle'. This might lead to -// rotations of a fractional bucket which is handled by linearly interpolating. -Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram, - const float angle) { +float MatchHistograms(const Eigen::VectorXf& submap_histogram, + const Eigen::VectorXf& scan_histogram) { + // We compute the dot product of normalized histograms as a measure of + // similarity. + const float scan_histogram_norm = scan_histogram.norm(); + const float submap_histogram_norm = submap_histogram.norm(); + const float normalization = scan_histogram_norm * submap_histogram_norm; + if (normalization < 1e-3f) { + return 1.f; + } + return submap_histogram.dot(scan_histogram) / normalization; +} + +} // namespace + +Eigen::VectorXf RotationalScanMatcher::RotateHistogram( + const Eigen::VectorXf& histogram, const float angle) { const float rotate_by_buckets = -angle * histogram.size() / M_PI; int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f); const float fraction = rotate_by_buckets - full_buckets; @@ -138,21 +151,6 @@ Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram, (1.f - fraction) * rotated_histogram_0; } -float MatchHistograms(const Eigen::VectorXf& submap_histogram, - const Eigen::VectorXf& scan_histogram) { - // We compute the dot product of normalized histograms as a measure of - // similarity. - const float scan_histogram_norm = scan_histogram.norm(); - const float submap_histogram_norm = submap_histogram.norm(); - const float normalization = scan_histogram_norm * submap_histogram_norm; - if (normalization < 1e-3f) { - return 1.f; - } - return submap_histogram.dot(scan_histogram) / normalization; -} - -} // namespace - Eigen::VectorXf RotationalScanMatcher::ComputeHistogram( const sensor::PointCloud& point_cloud, const int histogram_size) { Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size); @@ -166,6 +164,9 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram( return histogram; } +RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf& histogram) + : histogram_(histogram) {} + RotationalScanMatcher::RotationalScanMatcher( const std::vector>& histograms_at_angles) : histogram_( diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h index 51156f0..ac1a792 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h @@ -28,10 +28,18 @@ namespace scan_matching { class RotationalScanMatcher { public: + // Rotates the given 'histogram' by the given 'angle'. This might lead to + // rotations of a fractional bucket which is handled by linearly + // interpolating. + static Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram, + float angle); + // Computes the histogram for a gravity aligned 'point_cloud'. static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud, int histogram_size); + explicit RotationalScanMatcher(const Eigen::VectorXf& histogram); + // Creates a matcher from the given histograms rotated by the given angles. // The angles should be chosen to bring the histograms into approximately the // same frame. diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc index c84e677..ad1455c 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc @@ -28,7 +28,9 @@ namespace { TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) { Eigen::VectorXf histogram(7); histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f; - RotationalScanMatcher matcher({{histogram, 0.f}}); + const std::vector> histogram_at_angle = { + {histogram, 0.f}}; + RotationalScanMatcher matcher(histogram_at_angle); const auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f}); ASSERT_EQ(2, scores.size()); EXPECT_NEAR(1.f, scores[0], 1e-6); @@ -39,8 +41,9 @@ TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) { constexpr int kNumBuckets = 10; constexpr float kAnglePerBucket = M_PI / kNumBuckets; constexpr float kNoInitialRotation = 0.f; - RotationalScanMatcher matcher( - {{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}}); + const std::vector> histogram_at_angle = { + {Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}}; + RotationalScanMatcher matcher(histogram_at_angle); for (float t = 0.f; t < 1.f; t += 0.1f) { // 't' is the fraction of overlap and we have to divide by the norm of the // histogram to get the expected score.