Add IntensityHybridGrid. (#1739)

Adds a new structure IntensityHybridGrid, similar to HybridGrid
but which stores intensities instead of probabilities.
InterpolatedGrid is adapted to handle both types of HybridGrids.

Signed-off-by: Wolfgang Hess <whess@lyft.com>

cartographer/mapping/3d/hybrid_grid.h

@@ -544,6 +544,32 @@ class HybridGrid : public HybridGridBase<uint16> {
   std::vector<ValueType*> update_indices_;
 };
 
+struct AverageIntensityData {
+  float sum = 0.f;
+  int count = 0;
+};
+
+class IntensityHybridGrid : public HybridGridBase<AverageIntensityData> {
+ public:
+  explicit IntensityHybridGrid(const float resolution)
+      : HybridGridBase<AverageIntensityData>(resolution) {}
+
+  void AddIntensity(const Eigen::Array3i& index, const float intensity) {
+    AverageIntensityData* const cell = mutable_value(index);
+    cell->count += 1;
+    cell->sum += intensity;
+  }
+
+  float GetIntensity(const Eigen::Array3i& index) const {
+    const AverageIntensityData& cell = value(index);
+    if (cell.count == 0) {
+      return 0.f;
+    } else {
+      return cell.sum / cell.count;
+    }
+  }
+};
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping/3d/hybrid_grid_test.cc

@@ -84,6 +84,23 @@ TEST(HybridGridTest, GetProbability) {
   }
 }
 
+TEST(HybridGridTest, GetIntensity) {
+  IntensityHybridGrid hybrid_grid(1.f);
+  const Eigen::Array3i cell_index =
+      hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f));
+  const float intensity = 58.0f;
+
+  EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), 0.0f, 1e-9);
+  hybrid_grid.AddIntensity(cell_index, intensity);
+  EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), intensity, 1e-9);
+  for (const Eigen::Array3i& index :
+       {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)),
+        hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)),
+        hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) {
+    EXPECT_NEAR(hybrid_grid.GetIntensity(index), 0.0f, 1e-9);
+  }
+}
+
 MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); }
 
 TEST(HybridGridTest, GetCellIndex) {

cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h

@@ -25,22 +25,23 @@ namespace cartographer {
 namespace mapping {
 namespace scan_matching {
 
-// Interpolates between HybridGrid probability voxels. We use the tricubic
+// Interpolates between HybridGrid voxels. We use the tricubic
 // interpolation which interpolates the values and has vanishing derivative at
 // these points.
 //
 // This class is templated to work with the autodiff that Ceres provides.
 // For this reason, it is also important that the interpolation scheme be
 // continuously differentiable.
+template <class HybridGridType>
 class InterpolatedGrid {
  public:
-  explicit InterpolatedGrid(const HybridGrid& hybrid_grid)
+  explicit InterpolatedGrid(const HybridGridType& hybrid_grid)
       : hybrid_grid_(hybrid_grid) {}
 
-  InterpolatedGrid(const InterpolatedGrid&) = delete;
+  InterpolatedGrid(const InterpolatedGrid<HybridGridType>&) = delete;
-  InterpolatedGrid& operator=(const InterpolatedGrid&) = delete;
+  InterpolatedGrid& operator=(const InterpolatedGrid<HybridGridType>&) = delete;
 
-  // Returns the interpolated probability at (x, y, z) of the HybridGrid
+  // Returns the interpolated value at (x, y, z) of the HybridGrid
   // used to perform the interpolation.
   //
   // This is a piecewise, continuously differentiable function. We use the
@@ -48,27 +49,27 @@ class InterpolatedGrid {
   // tensor product volume of piecewise cubic polynomials that interpolate
   // the values, and have vanishing derivative at the interval boundaries.
   template <typename T>
-  T GetProbability(const T& x, const T& y, const T& z) const {
+  T GetInterpolatedValue(const T& x, const T& y, const T& z) const {
     double x1, y1, z1, x2, y2, z2;
     ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2);
 
     const Eigen::Array3i index1 =
         hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1));
-    const double q111 = hybrid_grid_.GetProbability(index1);
+    const double q111 = GetValue(hybrid_grid_, index1);
     const double q112 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1));
     const double q121 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0));
     const double q122 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1));
     const double q211 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0));
     const double q212 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1));
     const double q221 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0));
     const double q222 =
-        hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1));
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1));
 
     const T normalized_x = (x - x1) / (x2 - x1);
     const T normalized_y = (y - y1) / (y2 - y1);
@@ -145,9 +146,22 @@ class InterpolatedGrid {
     return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
   }
 
-  const HybridGrid& hybrid_grid_;
+  static float GetValue(const HybridGrid& probability_grid,
+                        const Eigen::Array3i& index) {
+    return probability_grid.GetProbability(index);
+  }
+
+  static float GetValue(const IntensityHybridGrid& intensity_grid,
+                        const Eigen::Array3i& index) {
+    return intensity_grid.GetIntensity(index);
+  }
+
+  const HybridGridType& hybrid_grid_;
 };
 
+using InterpolatedIntensityGrid = InterpolatedGrid<IntensityHybridGrid>;
+using InterpolatedProbabilityGrid = InterpolatedGrid<HybridGrid>;
+
 }  // namespace scan_matching
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc

@@ -44,7 +44,7 @@ class InterpolatedGridTest : public ::testing::Test {
   }
 
   HybridGrid hybrid_grid_;
-  InterpolatedGrid interpolated_grid_;
+  InterpolatedProbabilityGrid interpolated_grid_;
 };
 
 TEST_F(InterpolatedGridTest, InterpolatesGridPoints) {
@@ -52,7 +52,7 @@ TEST_F(InterpolatedGridTest, InterpolatesGridPoints) {
     for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) {
       for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) {
         EXPECT_NEAR(GetHybridGridProbability(x, y, z),
-                    interpolated_grid_.GetProbability(x, y, z), 1e-6);
+                    interpolated_grid_.GetInterpolatedValue(x, y, z), 1e-6);
       }
     }
   }
@@ -73,10 +73,11 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
         for (double sample = kSampleStep;
              sample < hybrid_grid_.resolution() - 2 * kSampleStep;
              sample += kSampleStep) {
-          EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability(
+          EXPECT_LT(0.,
-                                               x + sample + kSampleStep, y, z) -
+                    grid_difference * (interpolated_grid_.GetInterpolatedValue(
-                                           interpolated_grid_.GetProbability(
+                                           x + sample + kSampleStep, y, z) -
-                                               x + sample, y, z)));
+                                       interpolated_grid_.GetInterpolatedValue(
+                                           x + sample, y, z)));
         }
       }
     }

cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h

@@ -73,7 +73,7 @@ class OccupiedSpaceCostFunction3D {
       const Eigen::Matrix<T, 3, 1> world =
           transform * point_cloud_[i].position.cast<T>();
       const T probability =
-          interpolated_grid_.GetProbability(world[0], world[1], world[2]);
+          interpolated_grid_.GetInterpolatedValue(world[0], world[1], world[2]);
       residual[i] = scaling_factor_ * (1. - probability);
     }
     return true;
@@ -81,7 +81,7 @@ class OccupiedSpaceCostFunction3D {
 
   const double scaling_factor_;
   const sensor::PointCloud& point_cloud_;
-  const InterpolatedGrid interpolated_grid_;
+  const InterpolatedProbabilityGrid interpolated_grid_;
 };
 
 }  // namespace scan_matching