Add new sensor::PointCloud interface. (#1759)

These changes are necessary in order to later add intensities. Signed-off-by: Wolfgang Hess <whess@lyft.com>
cartographer-project · Oct 13, 2020 · ee98a92 · ee98a92
1 parent 8ac967a
commit ee98a92
Show file tree

Hide file tree

Showing 21 changed files with 155 additions and 84 deletions.
diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc
@@ -39,8 +39,8 @@ HybridGridPointsProcessor::FromDictionary(
 }
 
 void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
-  range_data_inserter_.Insert({batch->origin, batch->points, {}},
-                              &hybrid_grid_);
+  range_data_inserter_.Insert(
+      {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_);
   next_->Process(std::move(batch));
 }
 

diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc
@@ -21,7 +21,7 @@ namespace io {
 
 void RemovePoints(absl::flat_hash_set<int> to_remove, PointsBatch* batch) {
   const int new_num_points = batch->points.size() - to_remove.size();
-  sensor::PointCloud points;
+  std::vector<sensor::RangefinderPoint> points;
   points.reserve(new_num_points);
   std::vector<float> intensities;
   if (!batch->intensities.empty()) {

diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h
@@ -25,7 +25,7 @@
 #include "absl/container/flat_hash_set.h"
 #include "cartographer/common/time.h"
 #include "cartographer/io/color.h"
-#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/sensor/rangefinder_point.h"
 
 namespace cartographer {
 namespace io {
@@ -53,7 +53,7 @@ struct PointsBatch {
   int trajectory_id;
 
   // Geometry of the points in the map frame.
-  sensor::PointCloud points;
+  std::vector<sensor::RangefinderPoint> points;
 
   // Intensities are optional and may be unspecified. The meaning of these
   // intensity values varies by device. For example, the VLP16 provides values

diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc
@@ -126,8 +126,9 @@ ProbabilityGridPointsProcessor::FromDictionary(
 
 void ProbabilityGridPointsProcessor::Process(
     std::unique_ptr<PointsBatch> batch) {
-  range_data_inserter_.Insert({batch->origin, batch->points, {}},
-                              &probability_grid_);
+  range_data_inserter_.Insert(
+      {batch->origin, sensor::PointCloud(batch->points), {}},
+      &probability_grid_);
   next_->Process(std::move(batch));
 }
 

diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc
@@ -85,8 +85,9 @@ std::vector<char> CreateExpectedProbabilityGrid(
   mapping::ValueConversionTables conversion_tables;
   auto probability_grid = CreateProbabilityGrid(
       probability_grid_options->GetDouble("resolution"), &conversion_tables);
-  range_data_inserter.Insert({points_batch->origin, points_batch->points, {}},
-                             &probability_grid);
+  range_data_inserter.Insert(
+      {points_batch->origin, sensor::PointCloud(points_batch->points), {}},
+      &probability_grid);
 
   std::vector<char> probability_grid_proto(
       probability_grid.ToProto().ByteSize());

diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc
@@ -142,9 +142,11 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
   std::vector<float> normals;
   if (options_.project_sdf_distance_to_scan_normal() ||
       scale_update_weight_angle_scan_normal_to_ray) {
-    std::sort(sorted_range_data.returns.begin(),
-              sorted_range_data.returns.end(),
+    std::vector<sensor::RangefinderPoint> returns =
+        sorted_range_data.returns.points();
+    std::sort(returns.begin(), returns.end(),
               RangeDataSorter(sorted_range_data.origin));
+    sorted_range_data.returns = sensor::PointCloud(std::move(returns));
     normals = EstimateNormals(sorted_range_data,
                               options_.normal_estimation_options());
   }

diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc
@@ -41,12 +41,14 @@ class RangeDataInserter3DTest : public ::testing::Test {
 
   void InsertPointCloud() {
     const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
-    sensor::PointCloud 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, returns, {}},
-                                 &hybrid_grid_);
+    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_);
   }
 
   float GetProbability(float x, float y, float z) const {

diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc
@@ -45,9 +45,9 @@ TEST(NormalEstimation2DTest, SinglePoint) {
     const double angle = static_cast<double>(angle_idx) /
                              static_cast<double>(num_angles) * 2. * M_PI -
                          M_PI;
-    range_data.returns.clear();
-    range_data.returns.push_back(
-        {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
+    range_data.returns = sensor::PointCloud(
+        {{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}
+              .cast<float>()}});
     std::vector<float> normals;
     normals = EstimateNormals(range_data, options);
     EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI),
@@ -75,30 +75,27 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
     EXPECT_NEAR(normal, -M_PI_2, 1e-4);
   }
   normals.clear();
-  range_data.returns.clear();
-  range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
-  range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}});
-  range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
+  range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, 1.f, 0.f}},
+                                           {Eigen::Vector3f{1.f, 0.f, 0.f}},
+                                           {Eigen::Vector3f{1.f, -1.f, 0.f}}});
   normals = EstimateNormals(range_data, options);
   for (const float normal : normals) {
     EXPECT_NEAR(std::abs(normal), M_PI, 1e-4);
   }
 
   normals.clear();
-  range_data.returns.clear();
-  range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
-  range_data.returns.push_back({Eigen::Vector3f{0.f, -1.f, 0.f}});
-  range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
+  range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, -1.f, 0.f}},
+                                           {Eigen::Vector3f{0.f, -1.f, 0.f}},
+                                           {Eigen::Vector3f{-1.f, -1.f, 0.f}}});
   normals = EstimateNormals(range_data, options);
   for (const float normal : normals) {
     EXPECT_NEAR(normal, M_PI_2, 1e-4);
   }
 
   normals.clear();
-  range_data.returns.clear();
-  range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
-  range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}});
-  range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
+  range_data.returns = sensor::PointCloud({{Eigen::Vector3f{-1.f, -1.f, 0.f}},
+                                           {Eigen::Vector3f{-1.f, 0.f, 0.f}},
+                                           {Eigen::Vector3f{-1.f, 1.f, 0.f}}});
   normals = EstimateNormals(range_data, options);
   for (const float normal : normals) {
     EXPECT_NEAR(normal, 0, 1e-4);

diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc
@@ -33,7 +33,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
   ValueConversionTables conversion_tables;
   ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
                        &conversion_tables);
-  sensor::PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}};
+  sensor::PointCloud point_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}});
   ceres::Problem problem;
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid));
@@ -54,4 +54,4 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
 }  // namespace
 }  // namespace scan_matching
 }  // namespace mapping
-}  // namespace cartographer
+}  // namespace cartographer
diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc
@@ -72,7 +72,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test {
 };
 
 TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
-  const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}};
+  const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}});
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@@ -88,7 +88,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
 
 TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
   InsertPointcloud();
-  const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
+  const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}});
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@@ -108,7 +108,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
 
 TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
   InsertPointcloud();
-  sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
+  sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}});
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   std::array<double, 3> pose_estimate{{0., 0.1, 0.}};
@@ -139,7 +139,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
 
 TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) {
   InsertPointcloud();
-  sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
+  sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}});
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   std::array<double, 3> pose_estimate{{0., 0.4, 0.}};

diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc
@@ -40,13 +40,18 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
         options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {}
 
   void SetUp() override {
-    point_cloud_ = {
-        {Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 0.f, 0.f)},
-        {Eigen::Vector3f(5.f, 0.f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)},
-        {Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)},
-        {Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)},
-        {Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)},
-        {Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}};
+    point_cloud_ = sensor::PointCloud({{Eigen::Vector3f(4.f, 0.f, 0.f)},
+                                       {Eigen::Vector3f(4.5f, 0.f, 0.f)},
+                                       {Eigen::Vector3f(5.f, 0.f, 0.f)},
+                                       {Eigen::Vector3f(5.5f, 0.f, 0.f)},
+                                       {Eigen::Vector3f(0.f, 4.f, 0.f)},
+                                       {Eigen::Vector3f(0.f, 4.5f, 0.f)},
+                                       {Eigen::Vector3f(0.f, 5.f, 0.f)},
+                                       {Eigen::Vector3f(0.f, 5.5f, 0.f)},
+                                       {Eigen::Vector3f(0.f, 0.f, 4.f)},
+                                       {Eigen::Vector3f(0.f, 0.f, 4.5f)},
+                                       {Eigen::Vector3f(0.f, 0.f, 5.f)},
+                                       {Eigen::Vector3f(0.f, 0.f, 5.5f)}});
   }
 
   transform::Rigid3f GetRandomPose() {
@@ -159,7 +164,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
     const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
         low_resolution_result = fast_correlative_scan_matcher->Match(
             transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
-            CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}),
+            CreateConstantData(
+                sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})),
             kMinScore);
     EXPECT_THAT(low_resolution_result, testing::IsNull())
         << low_resolution_result->low_resolution_score;
@@ -188,7 +194,9 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
   const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
       low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
           Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
-          CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore);
+          CreateConstantData(
+              sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})),
+          kMinScore);
   EXPECT_THAT(low_resolution_result, testing::IsNull())
       << low_resolution_result->low_resolution_score;
 }

diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc
@@ -68,7 +68,7 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice,
   // will add the angle between points to the histogram with the maximum weight.
   // This is to reject, e.g., the angles observed on the ceiling and floor.
   const Eigen::Vector3f centroid = ComputeCentroid(slice);
-  Eigen::Vector3f last_point_position = slice.front().position;
+  Eigen::Vector3f last_point_position = slice.points().front().position;
   for (const sensor::RangefinderPoint& point : slice) {
     const Eigen::Vector2f delta =
         (point.position - last_point_position).head<2>();

diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc
@@ -33,11 +33,14 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
       common::FromUniversal(42),
       Eigen::Quaterniond(1., 2., -3., -4.),
       sensor::CompressedPointCloud(
-          {{Eigen::Vector3f{1.f, 2.f, 0.f}}, {Eigen::Vector3f{0.f, 0.f, 1.f}}})
+          sensor::PointCloud({{Eigen::Vector3f{1.f, 2.f, 0.f}},
+                              {Eigen::Vector3f{0.f, 0.f, 1.f}}}))
           .Decompress(),
-      sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}})
+      sensor::CompressedPointCloud(
+          sensor::PointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}}))
           .Decompress(),
-      sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}})
+      sensor::CompressedPointCloud(
+          sensor::PointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}}))
           .Decompress(),
       Eigen::VectorXf::Unit(20, 4),
       transform::Rigid3d({1., 2., 3.},
@@ -46,12 +49,12 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
   const TrajectoryNode::Data actual = FromProto(proto);
   EXPECT_EQ(expected.time, actual.time);
   EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment));
-  EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud,
-            actual.filtered_gravity_aligned_point_cloud);
-  EXPECT_EQ(expected.high_resolution_point_cloud,
-            actual.high_resolution_point_cloud);
-  EXPECT_EQ(expected.low_resolution_point_cloud,
-            actual.low_resolution_point_cloud);
+  EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud.points(),
+            actual.filtered_gravity_aligned_point_cloud.points());
+  EXPECT_EQ(expected.high_resolution_point_cloud.points(),
+            actual.high_resolution_point_cloud.points());
+  EXPECT_EQ(expected.low_resolution_point_cloud.points(),
+            actual.low_resolution_point_cloud.points());
   EXPECT_EQ(expected.rotational_scan_matcher_histogram,
             actual.rotational_scan_matcher_histogram);
   EXPECT_THAT(actual.local_pose,

diff --git a/cartographer/sensor/compressed_point_cloud_test.cc b/cartographer/sensor/compressed_point_cloud_test.cc
@@ -48,10 +48,10 @@ MATCHER_P(ApproximatelyEquals, expected,
 // Helper function to test the mapping of a single point. Includes test for
 // recompressing the same point again.
 void TestPoint(const Eigen::Vector3f& p) {
-  CompressedPointCloud compressed({{p}});
+  CompressedPointCloud compressed(PointCloud({{p}}));
   EXPECT_EQ(1, compressed.size());
   EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p));
-  CompressedPointCloud recompressed({*compressed.begin()});
+  CompressedPointCloud recompressed(PointCloud({*compressed.begin()}));
   EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p));
 }
 
@@ -70,18 +70,19 @@ TEST(CompressPointCloudTest, CompressesPointsCorrectly) {
 }
 
 TEST(CompressPointCloudTest, Compresses) {
-  const CompressedPointCloud compressed({{Eigen::Vector3f(0.838f, 0, 0)},
-                                         {Eigen::Vector3f(0.839f, 0, 0)},
-                                         {Eigen::Vector3f(0.840f, 0, 0)}});
+  const CompressedPointCloud compressed(
+      PointCloud({{Eigen::Vector3f(0.838f, 0, 0)},
+                  {Eigen::Vector3f(0.839f, 0, 0)},
+                  {Eigen::Vector3f(0.840f, 0, 0)}}));
   EXPECT_FALSE(compressed.empty());
   EXPECT_EQ(3, compressed.size());
   const PointCloud decompressed = compressed.Decompress();
   EXPECT_EQ(3, decompressed.size());
-  EXPECT_THAT(decompressed,
+  EXPECT_THAT(decompressed.points(),
               Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0))));
-  EXPECT_THAT(decompressed,
+  EXPECT_THAT(decompressed.points(),
               Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0))));
-  EXPECT_THAT(decompressed,
+  EXPECT_THAT(decompressed.points(),
               Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0))));
 }
 

diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc
@@ -126,12 +126,17 @@ std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
 
 }  // namespace
 
-PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
+std::vector<RangefinderPoint> VoxelFilter(
+    const std::vector<RangefinderPoint>& points, const float resolution) {
   return RandomizedVoxelFilter(
-      point_cloud, resolution,
+      points, resolution,
       [](const RangefinderPoint& point) { return point.position; });
 }
 
+PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
+  return PointCloud(VoxelFilter(point_cloud.points(), resolution));
+}
+
 TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
                             const float resolution) {
   return RandomizedVoxelFilter(

diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h
@@ -27,6 +27,8 @@
 namespace cartographer {
 namespace sensor {
 
+std::vector<RangefinderPoint> VoxelFilter(
+    const std::vector<RangefinderPoint>& points, const float resolution);
 PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution);
 TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
                             const float resolution);

diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc
@@ -33,9 +33,9 @@ TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
                                 {{0.f, 0.f, 0.1f}}});
   const PointCloud result = VoxelFilter(point_cloud, 0.3f);
   ASSERT_EQ(result.size(), 2);
-  EXPECT_THAT(point_cloud, Contains(result[0]));
-  EXPECT_THAT(point_cloud, Contains(result[1]));
-  EXPECT_THAT(result, Contains(point_cloud[2]));
+  EXPECT_THAT(point_cloud.points(), Contains(result[0]));
+  EXPECT_THAT(point_cloud.points(), Contains(result[1]));
+  EXPECT_THAT(result.points(), Contains(point_cloud[2]));
 }
 
 TEST(VoxelFilterTest, HandlesLargeCoordinates) {
@@ -45,7 +45,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) {
                                 {{-200000.f, 0.f, 0.f}}});
   const PointCloud result = VoxelFilter(point_cloud, 0.01f);
   EXPECT_EQ(result.size(), 2);
-  EXPECT_THAT(result, Contains(point_cloud[3]));
+  EXPECT_THAT(result.points(), Contains(point_cloud[3]));
 }
 
 TEST(VoxelFilterTest, IgnoresTime) {