From 4829ffee46c45b9e6970e040160d088a6cb57e6e Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 6 Sep 2017 10:59:16 +0200 Subject: [PATCH] Allow rotating histograms in the rotational scan matcher. (#501) This allows reusing histograms to match for different yaws. Adds a unit test to test that rotated histograms match as expected. --- .../fast_correlative_scan_matcher.cc | 26 ++++- .../scan_matching/rotational_scan_matcher.cc | 103 +++++++++--------- .../scan_matching/rotational_scan_matcher.h | 25 +++-- .../rotational_scan_matcher_test.cc | 71 ++++++++++++ 4 files changed, 163 insertions(+), 62 deletions(-) create mode 100644 cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc 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 8c01996..d0c3746 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -125,6 +125,24 @@ struct Candidate { bool operator>(const Candidate& other) const { return score > other.score; } }; +namespace { + +scan_matching::RotationalScanMatcher CreateRotationalScanMatcher( + const std::vector& nodes, + const int histogram_size) { + Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size); + for (const mapping::TrajectoryNode& node : nodes) { + histogram += scan_matching::RotationalScanMatcher::ComputeHistogram( + sensor::TransformPointCloud( + node.constant_data->range_data.returns.Decompress(), + node.pose.cast()), + histogram_size); + } + return scan_matching::RotationalScanMatcher({{histogram, 0.f}}); +} + +} // namespace + FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, const HybridGrid* const low_resolution_hybrid_grid, @@ -136,7 +154,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( precomputation_grid_stack_( common::make_unique(hybrid_grid, options)), low_resolution_hybrid_grid_(low_resolution_hybrid_grid), - rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {} + rotational_scan_matcher_(CreateRotationalScanMatcher( + nodes, options_.rotational_histogram_size())) {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} @@ -286,7 +305,10 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( angles.push_back(rz * angular_step_size); } const std::vector scores = rotational_scan_matcher_.Match( - sensor::TransformPointCloud(fine_point_cloud, initial_pose), angles); + RotationalScanMatcher::ComputeHistogram( + sensor::TransformPointCloud(fine_point_cloud, initial_pose), + options_.rotational_histogram_size()), + 0.f /* initial_angle */, angles); for (size_t i = 0; i != angles.size(); ++i) { if (scores[i] < options_.min_rotational_score()) { continue; diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc index f1fc09e..26a3385 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc @@ -20,6 +20,7 @@ #include #include "cartographer/common/math.h" +#include "cartographer/common/port.h" namespace cartographer { namespace mapping_3d { @@ -57,14 +58,8 @@ Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) { return sum / static_cast(slice.size()); } -struct AngleValuePair { - float angle; - float value; -}; - -void AddPointCloudSliceToValueVector( - const sensor::PointCloud& slice, - std::vector* value_vector) { +void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice, + Eigen::VectorXf* const histogram) { if (slice.empty()) { return; } @@ -88,7 +83,7 @@ void AddPointCloudSliceToValueVector( const float angle = common::atan2(delta); const float value = std::max( 0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized()))); - value_vector->push_back(AngleValuePair{angle, value}); + AddValueToHistogram(angle, value, histogram); } } @@ -122,68 +117,78 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) { return result; } -std::vector GetValuesForHistogram( - const sensor::PointCloud& point_cloud) { - std::map slices; - for (const Eigen::Vector3f& point : point_cloud) { - slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point); +// 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) { + 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; + while (full_buckets < 0) { + full_buckets += histogram.size(); } - std::vector result; - for (const auto& slice : slices) { - AddPointCloudSliceToValueVector(SortSlice(slice.second), &result); + Eigen::VectorXf rotated_histogram_0 = Eigen::VectorXf::Zero(histogram.size()); + Eigen::VectorXf rotated_histogram_1 = Eigen::VectorXf::Zero(histogram.size()); + for (int i = 0; i != histogram.size(); ++i) { + rotated_histogram_0[i] = histogram[(i + full_buckets) % histogram.size()]; + rotated_histogram_1[i] = + histogram[(i + 1 + full_buckets) % histogram.size()]; } - return result; + return fraction * rotated_histogram_1 + + (1.f - fraction) * rotated_histogram_0; } -void AddValuesToHistogram(const std::vector& value_vector, - const float rotation, Eigen::VectorXf* histogram) { - for (const AngleValuePair& pair : value_vector) { - AddValueToHistogram(pair.angle + rotation, pair.value, histogram); +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); + std::map slices; + for (const Eigen::Vector3f& point : point_cloud) { + slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point); + } + for (const auto& slice : slices) { + AddPointCloudSliceToHistogram(SortSlice(slice.second), &histogram); + } + return histogram; +} + RotationalScanMatcher::RotationalScanMatcher( - const std::vector& nodes, const int histogram_size) - : histogram_(Eigen::VectorXf::Zero(histogram_size)) { - for (const mapping::TrajectoryNode& node : nodes) { - AddValuesToHistogram( - GetValuesForHistogram(sensor::TransformPointCloud( - node.constant_data->range_data.returns.Decompress(), - node.pose.cast())), - 0.f, &histogram_); + const std::vector>& histograms_at_angles) + : histogram_( + Eigen::VectorXf::Zero(histograms_at_angles.at(0).first.size())) { + for (const auto& histogram_at_angle : histograms_at_angles) { + histogram_ += + RotateHistogram(histogram_at_angle.first, histogram_at_angle.second); } } std::vector RotationalScanMatcher::Match( - const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& histogram, const float initial_angle, const std::vector& angles) const { std::vector result; result.reserve(angles.size()); - const std::vector value_vector = - GetValuesForHistogram(point_cloud); for (const float angle : angles) { - Eigen::VectorXf scan_histogram = Eigen::VectorXf::Zero(histogram_.size()); - AddValuesToHistogram(value_vector, angle, &scan_histogram); - result.push_back(MatchHistogram(scan_histogram)); + const Eigen::VectorXf scan_histogram = + RotateHistogram(histogram, initial_angle + angle); + result.push_back(MatchHistograms(histogram_, scan_histogram)); } return result; } -float RotationalScanMatcher::MatchHistogram( - const Eigen::VectorXf& scan_histogram) const { - // We compute the dot product of normalized histograms as a measure of - // similarity. - const float scan_histogram_norm = scan_histogram.norm(); - const float histogram_norm = histogram_.norm(); - const float normalization = scan_histogram_norm * histogram_norm; - if (normalization < 1e-3f) { - return 1.f; - } - return histogram_.dot(scan_histogram) / normalization; -} - } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h index 26e8c3b..65e0013 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h @@ -20,7 +20,6 @@ #include #include "Eigen/Geometry" -#include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { @@ -29,21 +28,25 @@ namespace scan_matching { class RotationalScanMatcher { public: + // Computes the histogram for a gravity aligned 'point_cloud'. + static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud, + int histogram_size); + + // 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. explicit RotationalScanMatcher( - const std::vector& nodes, int histogram_size); + const std::vector>& + histograms_at_angles); - RotationalScanMatcher(const RotationalScanMatcher&) = delete; - RotationalScanMatcher& operator=(const RotationalScanMatcher&) = delete; - - // Scores how well a 'point_cloud' can be understood as rotated by certain - // 'angles' relative to the 'nodes'. Each angle results in a score between - // 0 (worst) and 1 (best). - std::vector Match(const sensor::PointCloud& point_cloud, + // Scores how well 'histogram' rotated by 'initial_angle' can be understood as + // further rotated by certain 'angles' relative to the 'nodes'. Each angle + // results in a score between 0 (worst) and 1 (best). + std::vector Match(const Eigen::VectorXf& histogram, + float initial_angle, const std::vector& angles) const; private: - float MatchHistogram(const Eigen::VectorXf& scan_histogram) const; - Eigen::VectorXf histogram_; }; diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc new file mode 100644 index 0000000..08ae9ad --- /dev/null +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc @@ -0,0 +1,71 @@ +/* + * 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/rotational_scan_matcher.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +TEST(RotationalScanMatcherTest, 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 auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f}); + ASSERT_EQ(2, scores.size()); + EXPECT_NEAR(1.f, scores[0], 1e-6); + EXPECT_GT(1.f, scores[1]); +} + +TEST(RotationalScanMatcherTest, InterpolatesAsExpected) { + constexpr int kNumBuckets = 10; + constexpr float kAnglePerBucket = M_PI / kNumBuckets; + constexpr float kNoInitialRotation = 0.f; + RotationalScanMatcher matcher( + {{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}}); + 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. + const float expected_score = t / std::hypot(t, 1 - t); + // We rotate the 't'-th fraction of a bucket into the matcher's histogram. + auto scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2), + kNoInitialRotation, {t * kAnglePerBucket}); + ASSERT_EQ(1, scores.size()); + EXPECT_NEAR(expected_score, scores[0], 1e-6); + // Also verify rotating out of a bucket. + scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2), + kNoInitialRotation, {(2 - t) * kAnglePerBucket}); + ASSERT_EQ(1, scores.size()); + EXPECT_NEAR(expected_score, scores[0], 1e-6); + // And into and out of a bucket with negative angle. + scores = + matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 4), kNoInitialRotation, + {-t * kAnglePerBucket, (t - 2) * kAnglePerBucket}); + ASSERT_EQ(2, scores.size()); + EXPECT_NEAR(expected_score, scores[0], 1e-6); + EXPECT_NEAR(expected_score, scores[1], 1e-6); + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer