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.

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<mapping::TrajectoryNode>& 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<float>()),
+        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<PrecomputationGridStack>(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<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
     angles.push_back(rz * angular_step_size);
   }
   const std::vector<float> 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;

cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc

@@ -20,6 +20,7 @@
 #include <vector>
 
 #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<float>(slice.size());
 }
 
-struct AngleValuePair {
-  float angle;
-  float value;
-};
-
-void AddPointCloudSliceToValueVector(
-    const sensor::PointCloud& slice,
-    std::vector<AngleValuePair>* 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<AngleValuePair> GetValuesForHistogram(
-    const sensor::PointCloud& point_cloud) {
-  std::map<int, sensor::PointCloud> 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<AngleValuePair> 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<AngleValuePair>& 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<int, sensor::PointCloud> 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<mapping::TrajectoryNode>& 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<float>())),
-        0.f, &histogram_);
+    const std::vector<std::pair<Eigen::VectorXf, float>>& 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<float> RotationalScanMatcher::Match(
-    const sensor::PointCloud& point_cloud,
+    const Eigen::VectorXf& histogram, const float initial_angle,
     const std::vector<float>& angles) const {
   std::vector<float> result;
   result.reserve(angles.size());
-  const std::vector<AngleValuePair> 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

cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h

@@ -20,7 +20,6 @@
 #include <vector>
 
 #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<mapping::TrajectoryNode>& nodes, int histogram_size);
+      const std::vector<std::pair<Eigen::VectorXf, float>>&
+          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<float> 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<float> Match(const Eigen::VectorXf& histogram,
+                           float initial_angle,
                            const std::vector<float>& angles) const;
 
  private:
-  float MatchHistogram(const Eigen::VectorXf& scan_histogram) const;
-
   Eigen::VectorXf histogram_;
 };
 

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 <cmath>
+
+#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