Add new sensor::PointCloud interface. (#1759)
These changes are necessary in order to later add intensities. Signed-off-by: Wolfgang Hess <whess@lyft.com>master
Wolfgang Hess
GitHub
parent
8ac967a50d
commit
ee98a92845
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.}};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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>();
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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))));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -22,14 +22,35 @@
|
|||
namespace cartographer {
|
||||
namespace sensor {
|
||||
|
||||
PointCloud::PointCloud() {}
|
||||
PointCloud::PointCloud(std::vector<PointCloud::PointType> points)
|
||||
: points_(std::move(points)) {}
|
||||
|
||||
size_t PointCloud::size() const { return points_.size(); }
|
||||
bool PointCloud::empty() const { return points_.empty(); }
|
||||
|
||||
const std::vector<PointCloud::PointType>& 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<RangefinderPoint> 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,
|
||||
|
|
|
|||
|
|
@ -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<RangefinderPoint>;
|
||||
// Stores 3D positions of points together with some additional data, e.g.
|
||||
// intensities.
|
||||
struct PointCloud {
|
||||
public:
|
||||
using PointType = RangefinderPoint;
|
||||
|
||||
PointCloud();
|
||||
explicit PointCloud(std::vector<PointType> 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<PointType>& points() const;
|
||||
const PointType& operator[](const size_t index) const;
|
||||
|
||||
// Iterator over the points in the point cloud.
|
||||
using ConstIterator = std::vector<PointType>::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<PointType> 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<RangefinderPoint>;
|
|||
// third entry is 0.f (and the fourth entry is time).
|
||||
using TimedPointCloud = std::vector<TimedRangefinderPoint>;
|
||||
|
||||
// TODO(wohe): Retained for cartographer_ros. To be removed once it is no
|
||||
// longer used there.
|
||||
struct PointCloudWithIntensities {
|
||||
TimedPointCloud points;
|
||||
std::vector<float> intensities;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ proto::RangeData ToProto(const RangeData& range_data) {
|
|||
}
|
||||
|
||||
RangeData FromProto(const proto::RangeData& proto) {
|
||||
PointCloud returns;
|
||||
std::vector<RangefinderPoint> 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<RangefinderPoint> 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue