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<std::pair<Eigen::VectorXf, float>>& 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.

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
+float MatchHistograms(const Eigen::VectorXf& submap_histogram,
-// rotations of a fractional bucket which is handled by linearly interpolating.
+                      const Eigen::VectorXf& scan_histogram) {
-Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,
+  // We compute the dot product of normalized histograms as a measure of
-                                const float angle) {
+  // 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<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
     : histogram_(

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.

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<std::pair<Eigen::VectorXf, float>> 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(
+  const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
-      {{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}});
+      {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.