Use intensities in `Submap3D` and `RangeDataInserter3D`. (#1765)

`Submap3D` now also stores a pointer to `IntensityHybridGrid`.
Adapted `RangeDataInserter3D` to also insert intensities.

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

cartographer/io/hybrid_grid_points_processor.cc

@@ -40,7 +40,8 @@ HybridGridPointsProcessor::FromDictionary(
 
 void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   range_data_inserter_.Insert(
-      {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_);
+      {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_,
+      /*intensity_hybrid_grid=*/nullptr);
   next_->Process(std::move(batch));
 }
 

cartographer/mapping/3d/range_data_inserter_3d.cc

@@ -51,6 +51,21 @@ void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
   }
 }
 
+void InsertIntensitiesIntoGrid(const sensor::PointCloud& returns,
+                               IntensityHybridGrid* intensity_hybrid_grid,
+                               const float intensity_threshold) {
+  if (returns.intensities().size() > 0) {
+    for (size_t i = 0; i < returns.size(); ++i) {
+      if (returns.intensities()[i] > intensity_threshold) {
+        continue;
+      }
+      const Eigen::Array3i hit_cell =
+          intensity_hybrid_grid->GetCellIndex(returns[i].position);
+      intensity_hybrid_grid->AddIntensity(hit_cell, returns.intensities()[i]);
+    }
+  }
+}
+
 }  // namespace
 
 proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(
@@ -62,6 +77,8 @@ proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(
       parameter_dictionary->GetDouble("miss_probability"));
   options.set_num_free_space_voxels(
       parameter_dictionary->GetInt("num_free_space_voxels"));
+  options.set_intensity_threshold(
+      parameter_dictionary->GetDouble("intensity_threshold"));
   CHECK_GT(options.hit_probability(), 0.5);
   CHECK_LT(options.miss_probability(), 0.5);
   return options;
@@ -75,9 +92,10 @@ RangeDataInserter3D::RangeDataInserter3D(
       miss_table_(
           ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {}
 
-void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
+void RangeDataInserter3D::Insert(
-                                 HybridGrid* hybrid_grid) const {
+    const sensor::RangeData& range_data, HybridGrid* hybrid_grid,
-  CHECK(hybrid_grid != nullptr);
+    IntensityHybridGrid* intensity_hybrid_grid) const {
+  CHECK_NOTNULL(hybrid_grid);
 
   for (const sensor::RangefinderPoint& hit : range_data.returns) {
     const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
@@ -88,6 +106,10 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
   // (i.e. no hits will be ignored because of a miss in the same cell).
   InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
                        hybrid_grid, options_.num_free_space_voxels());
+  if (intensity_hybrid_grid != nullptr) {
+    InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid,
+                              options_.intensity_threshold());
+  }
   hybrid_grid->FinishUpdate();
 }
 

cartographer/mapping/3d/range_data_inserter_3d.h

@@ -36,9 +36,10 @@ class RangeDataInserter3D {
   RangeDataInserter3D(const RangeDataInserter3D&) = delete;
   RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete;
 
-  // Inserts 'range_data' into 'hybrid_grid'.
+  // Inserts 'range_data' into 'hybrid_grid' and optionally into
-  void Insert(const sensor::RangeData& range_data,
+  // 'intensity_hybrid_grid'.
-              HybridGrid* hybrid_grid) const;
+  void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid,
+              IntensityHybridGrid* intensity_hybrid_grid) const;
 
  private:
   const proto::RangeDataInserterOptions3D options_;

cartographer/mapping/3d/range_data_inserter_3d_test.cc

@@ -28,12 +28,13 @@ namespace {
 
 class RangeDataInserter3DTest : public ::testing::Test {
  protected:
-  RangeDataInserter3DTest() : hybrid_grid_(1.f) {
+  RangeDataInserter3DTest() : hybrid_grid_(1.f), intensity_hybrid_grid_(1.f) {
     auto parameter_dictionary = common::MakeDictionary(
         "return { "
         "hit_probability = 0.7, "
         "miss_probability = 0.4, "
         "num_free_space_voxels = 1000, "
+        "intensity_threshold = 100, "
         "}");
     options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get());
     range_data_inserter_.reset(new RangeDataInserter3D(options_));
@@ -41,14 +42,28 @@ class RangeDataInserter3DTest : public ::testing::Test {
 
   void InsertPointCloud() {
     const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
-    std::vector<sensor::RangefinderPoint> returns = {
+    const std::vector<sensor::RangefinderPoint> returns = {
         {Eigen::Vector3f{-3.f, -1.f, 4.f}},
         {Eigen::Vector3f{-2.f, 0.f, 4.f}},
         {Eigen::Vector3f{-1.f, 1.f, 4.f}},
         {Eigen::Vector3f{0.f, 2.f, 4.f}}};
     range_data_inserter_->Insert(
         sensor::RangeData{origin, sensor::PointCloud(returns), {}},
-        &hybrid_grid_);
+        &hybrid_grid_,
+        /*intensity_hybrid_grid=*/nullptr);
+  }
+
+  void InsertPointCloudWithIntensities() {
+    const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
+    const std::vector<sensor::RangefinderPoint> returns = {
+        {Eigen::Vector3f{-3.f, -1.f, 4.f}},
+        {Eigen::Vector3f{-2.f, 0.f, 4.f}},
+        {Eigen::Vector3f{-1.f, 1.f, 4.f}},
+        {Eigen::Vector3f{0.f, 2.f, 4.f}}};
+    const std::vector<float> intensities{7.f, 8.f, 9.f, 10.f};
+    range_data_inserter_->Insert(
+        sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}},
+        &hybrid_grid_, &intensity_hybrid_grid_);
   }
 
   float GetProbability(float x, float y, float z) const {
@@ -56,6 +71,11 @@ class RangeDataInserter3DTest : public ::testing::Test {
         hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
   }
 
+  float GetIntensity(float x, float y, float z) const {
+    return intensity_hybrid_grid_.GetIntensity(
+        intensity_hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
+  }
+
   float IsKnown(float x, float y, float z) const {
     return hybrid_grid_.IsKnown(
         hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
@@ -65,6 +85,7 @@ class RangeDataInserter3DTest : public ::testing::Test {
 
  private:
   HybridGrid hybrid_grid_;
+  IntensityHybridGrid intensity_hybrid_grid_;
   std::unique_ptr<RangeDataInserter3D> range_data_inserter_;
   proto::RangeDataInserterOptions3D options_;
 };
@@ -89,6 +110,28 @@ TEST_F(RangeDataInserter3DTest, InsertPointCloud) {
   }
 }
 
+TEST_F(RangeDataInserter3DTest, InsertPointCloudWithIntensities) {
+  InsertPointCloudWithIntensities();
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f),
+              1e-4);
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
+              1e-4);
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f),
+              1e-4);
+  for (int x = -4; x <= 4; ++x) {
+    for (int y = -4; y <= 4; ++y) {
+      if (x < -3 || x > 0 || y != x + 2) {
+        EXPECT_FALSE(IsKnown(x, y, 4.f));
+        EXPECT_NEAR(0.f, GetIntensity(x, y, 4.f), 1e-6);
+      } else {
+        EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f),
+                    1e-4);
+        EXPECT_NEAR(10 + x, GetIntensity(x, y, 4.f), 1e-6);
+      }
+    }
+  }
+}
+
 TEST_F(RangeDataInserter3DTest, ProbabilityProgression) {
   InsertPointCloud();
   EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f),

cartographer/mapping/3d/submap_3d.cc

@@ -202,6 +202,8 @@ Submap3D::Submap3D(const float high_resolution, const float low_resolution,
           absl::make_unique<HybridGrid>(high_resolution)),
       low_resolution_hybrid_grid_(
           absl::make_unique<HybridGrid>(low_resolution)),
+      high_resolution_intensity_hybrid_grid_(
+          absl::make_unique<IntensityHybridGrid>(high_resolution)),
       rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {}
 
 Submap3D::Submap3D(const proto::Submap3D& proto)
@@ -278,9 +280,11 @@ void Submap3D::InsertData(const sensor::RangeData& range_data_in_local,
   range_data_inserter.Insert(
       FilterRangeDataByMaxRange(transformed_range_data,
                                 high_resolution_max_range),
-      high_resolution_hybrid_grid_.get());
+      high_resolution_hybrid_grid_.get(),
+      high_resolution_intensity_hybrid_grid_.get());
   range_data_inserter.Insert(transformed_range_data,
-                             low_resolution_hybrid_grid_.get());
+                             low_resolution_hybrid_grid_.get(),
+                             /*intensity_hybrid_grid=*/nullptr);
   set_num_range_data(num_range_data() + 1);
   const float yaw_in_submap_from_gravity = transform::GetYaw(
       local_pose().inverse().rotation() * local_from_gravity_aligned);
@@ -332,6 +336,11 @@ void ActiveSubmaps3D::AddSubmap(
     // This will crop the finished Submap before inserting a new Submap to
     // reduce peak memory usage a bit.
     CHECK(submaps_.front()->insertion_finished());
+    // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use
+    // active submaps and their associated intensity hybrid grids for scan
+    // matching, we call `ForgetIntensityHybridGrid` once we remove the submap
+    // from active submaps and no longer need the intensity hybrid grid.
+    submaps_.front()->ForgetIntensityHybridGrid();
     submaps_.erase(submaps_.begin());
   }
   const Eigen::VectorXf initial_rotational_scan_matcher_histogram =

cartographer/mapping/3d/submap_3d.h

@@ -60,6 +60,14 @@ class Submap3D : public Submap {
   const HybridGrid& low_resolution_hybrid_grid() const {
     return *low_resolution_hybrid_grid_;
   }
+  const IntensityHybridGrid& high_resolution_intensity_hybrid_grid() const {
+    CHECK(high_resolution_intensity_hybrid_grid_ != nullptr);
+    return *high_resolution_intensity_hybrid_grid_;
+  }
+  void ForgetIntensityHybridGrid() {
+    high_resolution_intensity_hybrid_grid_.reset();
+  }
+
   const Eigen::VectorXf& rotational_scan_matcher_histogram() const {
     return rotational_scan_matcher_histogram_;
   }
@@ -79,6 +87,7 @@ class Submap3D : public Submap {
 
   std::unique_ptr<HybridGrid> high_resolution_hybrid_grid_;
   std::unique_ptr<HybridGrid> low_resolution_hybrid_grid_;
+  std::unique_ptr<IntensityHybridGrid> high_resolution_intensity_hybrid_grid_;
   Eigen::VectorXf rotational_scan_matcher_histogram_;
 };
 

cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc

@@ -129,6 +129,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
               hit_probability = 0.7,
               miss_probability = 0.4,
               num_free_space_voxels = 0,
+              intensity_threshold = 100.0,
             },
           },
         }

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

@@ -94,6 +94,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
         "hit_probability = 0.7, "
         "miss_probability = 0.4, "
         "num_free_space_voxels = 5, "
+        "intensity_threshold = 100.0, "
         "}");
     return CreateRangeDataInserterOptions3D(parameter_dictionary.get());
   }
@@ -106,7 +107,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
         sensor::RangeData{pose.translation(),
                           sensor::TransformPointCloud(point_cloud_, pose),
                           {}},
-        hybrid_grid_.get());
+        hybrid_grid_.get(),
+        /*intensity_hybrid_grid=*/nullptr);
     hybrid_grid_->FinishUpdate();
 
     return absl::make_unique<FastCorrelativeScanMatcher3D>(

cartographer/mapping/proto/range_data_inserter_options_3d.proto

@@ -28,4 +28,7 @@ message RangeDataInserterOptions3D {
   // Up to how many free space voxels are updated for scan matching.
   // 0 disables free space.
   int32 num_free_space_voxels = 3;
+
+  // Do not insert intensities above this threshold into IntensityHybridGrid.
+  float intensity_threshold = 4;
 }

configuration_files/trajectory_builder_3d.lua

@@ -13,6 +13,7 @@
 -- limitations under the License.
 
 MAX_3D_RANGE = 60.
+INTENSITY_THRESHOLD = 40
 
 TRAJECTORY_BUILDER_3D = {
   min_range = 1.,
@@ -96,6 +97,7 @@ TRAJECTORY_BUILDER_3D = {
       hit_probability = 0.55,
       miss_probability = 0.49,
       num_free_space_voxels = 2,
+      intensity_threshold = INTENSITY_THRESHOLD,
     },
   },
 }