Take intensities into account in PointCloud transforms and filters. (#…

…1763)

Adds a new method to sensor::PointCloud, copy_if, which copies all
points satisfying a provided condition, together with the associated
intensities (if they exist).

Adapts transforms/filters to use this new method.

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

cartographer/mapping/3d/submap_3d.cc

@@ -37,16 +37,14 @@ struct PixelData {
 };
 
 // Filters 'range_data', retaining only the returns that have no more than
-// 'max_range' distance from the origin. Removes misses and reflectivity
-// information.
+// 'max_range' distance from the origin. Removes misses.
 sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
                                             const float max_range) {
   sensor::RangeData result{range_data.origin, {}, {}};
-  for (const sensor::RangefinderPoint& hit : range_data.returns) {
-    if ((hit.position - range_data.origin).norm() <= max_range) {
-      result.returns.push_back(hit);
-    }
-  }
+  result.returns =
+      range_data.returns.copy_if([&](const sensor::RangefinderPoint& point) {
+        return (point.position - range_data.origin).norm() <= max_range;
+      });
   return result;
 }
 

cartographer/sensor/internal/voxel_filter.cc

@@ -30,13 +30,9 @@ namespace {
 
 PointCloud FilterByMaxRange(const PointCloud& point_cloud,
                             const float max_range) {
-  PointCloud result;
-  for (const RangefinderPoint& point : point_cloud) {
-    if (point.position.norm() <= max_range) {
-      result.push_back(point);
-    }
-  }
-  return result;
+  return point_cloud.copy_if([max_range](const RangefinderPoint& point) {
+    return point.position.norm() <= max_range;
+  });
 }
 
 PointCloud AdaptivelyVoxelFiltered(

cartographer/sensor/point_cloud.cc

@@ -60,7 +60,7 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
   for (const RangefinderPoint& point : point_cloud.points()) {
     points.emplace_back(transform * point);
   }
-  return PointCloud(points);
+  return PointCloud(points, point_cloud.intensities());
 }
 
 TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
@@ -75,13 +75,9 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
 
 PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
                           const float max_z) {
-  PointCloud cropped_point_cloud;
-  for (const RangefinderPoint& point : point_cloud) {
-    if (min_z <= point.position.z() && point.position.z() <= max_z) {
-      cropped_point_cloud.push_back(point);
-    }
-  }
-  return cropped_point_cloud;
+  return point_cloud.copy_if([min_z, max_z](const RangefinderPoint& point) {
+    return min_z <= point.position.z() && point.position.z() <= max_z;
+  });
 }
 
 }  // namespace sensor

cartographer/sensor/point_cloud.h

@@ -54,6 +54,35 @@ class PointCloud {
 
   void push_back(PointType value);
 
+  // Creates a PointCloud consisting of all the points for which `predicate`
+  // returns true, together with the corresponding intensities.
+  template <class UnaryPredicate>
+  PointCloud copy_if(UnaryPredicate predicate) const {
+    std::vector<PointType> points;
+    std::vector<float> intensities;
+
+    // Note: benchmarks show that it is better to have this conditional outside
+    // the loop.
+    if (intensities_.empty()) {
+      for (size_t index = 0; index < size(); ++index) {
+        const PointType& point = points_[index];
+        if (predicate(point)) {
+          points.push_back(point);
+        }
+      }
+    } else {
+      for (size_t index = 0; index < size(); ++index) {
+        const PointType& point = points_[index];
+        if (predicate(point)) {
+          points.push_back(point);
+          intensities.push_back(intensities_[index]);
+        }
+      }
+    }
+
+    return PointCloud(points, intensities);
+  }
+
  private:
   // For 2D points, the third entry is 0.f.
   std::vector<PointType> points_;

cartographer/sensor/point_cloud_test.cc

@@ -19,12 +19,17 @@
 #include <cmath>
 
 #include "cartographer/transform/transform.h"
+#include "gmock/gmock.h"
 #include "gtest/gtest.h"
 
 namespace cartographer {
 namespace sensor {
 namespace {
 
+using ::testing::ElementsAre;
+using ::testing::FloatNear;
+using ::testing::IsEmpty;
+
 TEST(PointCloudTest, TransformPointCloud) {
   PointCloud point_cloud;
   point_cloud.push_back({{Eigen::Vector3f{0.5f, 0.5f, 1.f}}});
@@ -49,6 +54,38 @@ TEST(PointCloudTest, TransformTimedPointCloud) {
   EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
 }
 
+TEST(PointCloudTest, CopyIf) {
+  std::vector<RangefinderPoint> points = {
+      {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}};
+
+  const PointCloud point_cloud(points);
+  const PointCloud copied_point_cloud = point_cloud.copy_if(
+      [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; });
+
+  EXPECT_EQ(copied_point_cloud.size(), 2);
+  EXPECT_THAT(copied_point_cloud.intensities(), IsEmpty());
+  EXPECT_THAT(copied_point_cloud.points(),
+              ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}},
+                          RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}}));
+}
+
+TEST(PointCloudTest, CopyIfWithIntensities) {
+  std::vector<RangefinderPoint> points = {
+      {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}};
+  std::vector<float> intensities = {0.f, 1.f, 2.f};
+
+  const PointCloud point_cloud(points, intensities);
+  const PointCloud copied_point_cloud = point_cloud.copy_if(
+      [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; });
+  EXPECT_EQ(copied_point_cloud.size(), 2);
+  EXPECT_EQ(copied_point_cloud.intensities().size(), 2);
+  EXPECT_THAT(copied_point_cloud.points(),
+              ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}},
+                          RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}}));
+  EXPECT_THAT(copied_point_cloud.intensities(),
+              ElementsAre(FloatNear(1.f, 1e-6), FloatNear(2.f, 1e-6)));
+}
+
 }  // namespace
 }  // namespace sensor
 }  // namespace cartographer