diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index 32525fb..a573498 100644 --- 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 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 index ea0fe4c..cb4848c 100644 --- a/cartographer/io/points_batch.cc +++ b/cartographer/io/points_batch.cc @@ -21,7 +21,7 @@ namespace io { void RemovePoints(absl::flat_hash_set to_remove, PointsBatch* batch) { const int new_num_points = batch->points.size() - to_remove.size(); - sensor::PointCloud points; + std::vector points; points.reserve(new_num_points); std::vector intensities; if (!batch->intensities.empty()) { diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index 752966e..cc1d6bc 100644 --- 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 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 index 1d746f9..907b995 100644 --- 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 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 index 99753bb..8c48f6d 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -85,8 +85,9 @@ std::vector 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 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 index d7619d0..7a6a4e7 100644 --- 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 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 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 index eb051f1..0abc75a 100644 --- 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 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 index e32dcb4..bb6c453 100644 --- 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(angle_idx) / static_cast(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()}); + range_data.returns = sensor::PointCloud( + {{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.} + .cast()}}); std::vector 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 index 4cc82e2..c9ef182 100644 --- 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 cost_function( CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); @@ -54,4 +54,4 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { } // namespace } // namespace scan_matching } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // 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 index 5c6b584..3311ace 100644 --- 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 cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array 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 cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array 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 cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array 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 cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array 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 index 100a72f..85ea48a 100644 --- 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 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 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 index 0e4b5e0..a584ff4 100644 --- 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 index 029681c..c927a8d 100644 --- 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 index 341ad78..7dcd6bc 100644 --- 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 index d7c9028..67f2814 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -126,12 +126,17 @@ std::vector RandomizedVoxelFilter(const std::vector& point_cloud, } // namespace -PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { +std::vector VoxelFilter( + const std::vector& 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 index fd0c647..a99f861 100644 --- a/cartographer/sensor/internal/voxel_filter.h +++ b/cartographer/sensor/internal/voxel_filter.h @@ -27,6 +27,8 @@ namespace cartographer { namespace sensor { +std::vector VoxelFilter( + const std::vector& 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 index 4c18f1e..db6cd3f 100644 --- 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) { diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 959017c..01a1416 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -22,14 +22,35 @@ namespace cartographer { namespace sensor { +PointCloud::PointCloud() {} +PointCloud::PointCloud(std::vector points) + : points_(std::move(points)) {} + +size_t PointCloud::size() const { return points_.size(); } +bool PointCloud::empty() const { return points_.empty(); } + +const std::vector& PointCloud::points() const { + return points_; +} +const PointCloud::PointType& PointCloud::operator[](const size_t index) const { + return points_[index]; +} + +PointCloud::ConstIterator PointCloud::begin() const { return points_.begin(); } +PointCloud::ConstIterator PointCloud::end() const { return points_.end(); } + +void PointCloud::push_back(PointCloud::PointType value) { + points_.push_back(std::move(value)); +} + PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform) { - PointCloud result; - result.reserve(point_cloud.size()); - for (const RangefinderPoint& point : point_cloud) { - result.emplace_back(transform * point); + std::vector points; + points.reserve(point_cloud.size()); + for (const RangefinderPoint& point : point_cloud.points()) { + points.emplace_back(transform * point); } - return result; + return PointCloud(points); } TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index d2c338d..f20307f 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -28,9 +28,34 @@ namespace cartographer { namespace sensor { -// Stores 3D positions of points. -// For 2D points, the third entry is 0.f. -using PointCloud = std::vector; +// Stores 3D positions of points together with some additional data, e.g. +// intensities. +struct PointCloud { + public: + using PointType = RangefinderPoint; + + PointCloud(); + explicit PointCloud(std::vector points); + + // Returns the number of points in the point cloud. + size_t size() const; + // Checks whether there are any points in the point cloud. + bool empty() const; + + const std::vector& points() const; + const PointType& operator[](const size_t index) const; + + // Iterator over the points in the point cloud. + using ConstIterator = std::vector::const_iterator; + ConstIterator begin() const; + ConstIterator end() const; + + void push_back(PointType value); + + private: + // For 2D points, the third entry is 0.f. + std::vector points_; +}; // Stores 3D positions of points with their relative measurement time in the // fourth entry. Time is in seconds, increasing and relative to the moment when @@ -39,6 +64,8 @@ using PointCloud = std::vector; // third entry is 0.f (and the fourth entry is time). using TimedPointCloud = std::vector; +// TODO(wohe): Retained for cartographer_ros. To be removed once it is no +// longer used there. struct PointCloudWithIntensities { TimedPointCloud points; std::vector intensities; diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 4f547fe..39cc05f 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -27,8 +27,8 @@ namespace { TEST(PointCloudTest, TransformPointCloud) { PointCloud point_cloud; - point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}}); - point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}}); + point_cloud.push_back({{Eigen::Vector3f{0.5f, 0.5f, 1.f}}}); + point_cloud.push_back({{Eigen::Vector3f{3.5f, 0.5f, 42.f}}}); const PointCloud transformed_point_cloud = TransformPointCloud( point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 1151545..c30e68d 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -53,7 +53,7 @@ proto::RangeData ToProto(const RangeData& range_data) { } RangeData FromProto(const proto::RangeData& proto) { - PointCloud returns; + std::vector returns; if (proto.returns_size() > 0) { returns.reserve(proto.returns().size()); for (const auto& point_proto : proto.returns()) { @@ -65,7 +65,7 @@ RangeData FromProto(const proto::RangeData& proto) { returns.push_back({transform::ToEigen(point_proto)}); } } - PointCloud misses; + std::vector misses; if (proto.misses_size() > 0) { misses.reserve(proto.misses().size()); for (const auto& point_proto : proto.misses()) { @@ -77,7 +77,8 @@ RangeData FromProto(const proto::RangeData& proto) { misses.push_back({transform::ToEigen(point_proto)}); } } - return RangeData{transform::ToEigen(proto.origin()), returns, misses}; + return RangeData{transform::ToEigen(proto.origin()), PointCloud(returns), + PointCloud(misses)}; } } // namespace sensor