12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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 2020-10-13 15:57:45 +02:00 committed by GitHub
parent 8ac967a50d
commit ee98a92845
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
21 changed files with 155 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/io/hybrid_grid_points_processor.cc
View File

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

2
cartographer/io/points_batch.cc
View File

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

4
cartographer/io/points_batch.h
View File

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

5
cartographer/io/probability_grid_points_processor.cc
View File

@ -126,8 +126,9 @@ ProbabilityGridPointsProcessor::FromDictionary(
void ProbabilityGridPointsProcessor::Process( void ProbabilityGridPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) { std::unique_ptr<PointsBatch> batch) {
range_data_inserter_.Insert({batch->origin, batch->points, {}}, range_data_inserter_.Insert(
&probability_grid_); {batch->origin, sensor::PointCloud(batch->points), {}},
&probability_grid_);
next_->Process(std::move(batch)); next_->Process(std::move(batch));
} }

5
cartographer/io/probability_grid_points_processor_test.cc
View File

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

6
cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc
View File

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

14
cartographer/mapping/3d/range_data_inserter_3d_test.cc
View File

@ -41,12 +41,14 @@ class RangeDataInserter3DTest : public ::testing::Test {
void InsertPointCloud() { void InsertPointCloud() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}}, std::vector<sensor::RangefinderPoint> returns = {
{Eigen::Vector3f{-2.f, 0.f, 4.f}}, {Eigen::Vector3f{-3.f, -1.f, 4.f}},
{Eigen::Vector3f{-1.f, 1.f, 4.f}}, {Eigen::Vector3f{-2.f, 0.f, 4.f}},
{Eigen::Vector3f{0.f, 2.f, 4.f}}}; {Eigen::Vector3f{-1.f, 1.f, 4.f}},
range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}}, {Eigen::Vector3f{0.f, 2.f, 4.f}}};
&hybrid_grid_); range_data_inserter_->Insert(
sensor::RangeData{origin, sensor::PointCloud(returns), {}},
&hybrid_grid_);
} }
float GetProbability(float x, float y, float z) const { float GetProbability(float x, float y, float z) const {

27
cartographer/mapping/internal/2d/normal_estimation_2d_test.cc
View File

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

2
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc
View File

@ -33,7 +33,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
&conversion_tables); &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; ceres::Problem problem;
std::unique_ptr<ceres::CostFunction> cost_function( std::unique_ptr<ceres::CostFunction> cost_function(
CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid));

8
cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc
View File

@ -72,7 +72,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test {
}; };
TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { 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( std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
const std::array<double, 3> pose_estimate{{0., 0., 0.}}; const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@ -88,7 +88,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
InsertPointcloud(); 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( std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
const std::array<double, 3> pose_estimate{{0., 0., 0.}}; const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@ -108,7 +108,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
InsertPointcloud(); 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( std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
std::array<double, 3> pose_estimate{{0., 0.1, 0.}}; std::array<double, 3> pose_estimate{{0., 0.1, 0.}};
@ -139,7 +139,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) {
InsertPointcloud(); 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( std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
std::array<double, 3> pose_estimate{{0., 0.4, 0.}}; std::array<double, 3> pose_estimate{{0., 0.4, 0.}};

26
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc
View File

@ -40,13 +40,18 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {} options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {}
void SetUp() override { void SetUp() override {
point_cloud_ = { point_cloud_ = sensor::PointCloud({{Eigen::Vector3f(4.f, 0.f, 0.f)},
{Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 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(5.f, 0.f, 0.f)},
{Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)},
{Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)}, {Eigen::Vector3f(0.f, 4.f, 0.f)},
{Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)},
{Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}}; {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() { transform::Rigid3f GetRandomPose() {
@ -159,7 +164,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
low_resolution_result = fast_correlative_scan_matcher->Match( low_resolution_result = fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), 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); kMinScore);
EXPECT_THAT(low_resolution_result, testing::IsNull()) EXPECT_THAT(low_resolution_result, testing::IsNull())
<< low_resolution_result->low_resolution_score; << low_resolution_result->low_resolution_score;
@ -188,7 +194,9 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), 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()) EXPECT_THAT(low_resolution_result, testing::IsNull())
<< low_resolution_result->low_resolution_score; << low_resolution_result->low_resolution_score;
} }

2
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc
View File

@ -68,7 +68,7 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice,
// will add the angle between points to the histogram with the maximum weight. // 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. // This is to reject, e.g., the angles observed on the ceiling and floor.
const Eigen::Vector3f centroid = ComputeCentroid(slice); 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) { for (const sensor::RangefinderPoint& point : slice) {
const Eigen::Vector2f delta = const Eigen::Vector2f delta =
(point.position - last_point_position).head<2>(); (point.position - last_point_position).head<2>();

21
cartographer/mapping/trajectory_node_test.cc
View File

@ -33,11 +33,14 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
common::FromUniversal(42), common::FromUniversal(42),
Eigen::Quaterniond(1., 2., -3., -4.), Eigen::Quaterniond(1., 2., -3., -4.),
sensor::CompressedPointCloud( 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(), .Decompress(),
sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}}) sensor::CompressedPointCloud(
sensor::PointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}}))
.Decompress(), .Decompress(),
sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}}) sensor::CompressedPointCloud(
sensor::PointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}}))
.Decompress(), .Decompress(),
Eigen::VectorXf::Unit(20, 4), Eigen::VectorXf::Unit(20, 4),
transform::Rigid3d({1., 2., 3.}, transform::Rigid3d({1., 2., 3.},
@ -46,12 +49,12 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
const TrajectoryNode::Data actual = FromProto(proto); const TrajectoryNode::Data actual = FromProto(proto);
EXPECT_EQ(expected.time, actual.time); EXPECT_EQ(expected.time, actual.time);
EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment)); EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment));
EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud, EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud.points(),
actual.filtered_gravity_aligned_point_cloud); actual.filtered_gravity_aligned_point_cloud.points());
EXPECT_EQ(expected.high_resolution_point_cloud, EXPECT_EQ(expected.high_resolution_point_cloud.points(),
actual.high_resolution_point_cloud); actual.high_resolution_point_cloud.points());
EXPECT_EQ(expected.low_resolution_point_cloud, EXPECT_EQ(expected.low_resolution_point_cloud.points(),
actual.low_resolution_point_cloud); actual.low_resolution_point_cloud.points());
EXPECT_EQ(expected.rotational_scan_matcher_histogram, EXPECT_EQ(expected.rotational_scan_matcher_histogram,
actual.rotational_scan_matcher_histogram); actual.rotational_scan_matcher_histogram);
EXPECT_THAT(actual.local_pose, EXPECT_THAT(actual.local_pose,

17
cartographer/sensor/compressed_point_cloud_test.cc
View File

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

9
cartographer/sensor/internal/voxel_filter.cc
View File

@ -126,12 +126,17 @@ std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
} // namespace } // namespace
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { std::vector<RangefinderPoint> VoxelFilter(
const std::vector<RangefinderPoint>& points, const float resolution) {
return RandomizedVoxelFilter( return RandomizedVoxelFilter(
point_cloud, resolution, points, resolution,
[](const RangefinderPoint& point) { return point.position; }); [](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, TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
const float resolution) { const float resolution) {
return RandomizedVoxelFilter( return RandomizedVoxelFilter(

2
cartographer/sensor/internal/voxel_filter.h
View File

@ -27,6 +27,8 @@
namespace cartographer { namespace cartographer {
namespace sensor { namespace sensor {
std::vector<RangefinderPoint> VoxelFilter(
const std::vector<RangefinderPoint>& points, const float resolution);
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution); PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution);
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
const float resolution); const float resolution);

8
cartographer/sensor/internal/voxel_filter_test.cc
View File

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

31
cartographer/sensor/point_cloud.cc
View File

@ -22,14 +22,35 @@
namespace cartographer { namespace cartographer {
namespace sensor { 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, PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) { const transform::Rigid3f& transform) {
PointCloud result; std::vector<RangefinderPoint> points;
result.reserve(point_cloud.size()); points.reserve(point_cloud.size());
for (const RangefinderPoint& point : point_cloud) { for (const RangefinderPoint& point : point_cloud.points()) {
result.emplace_back(transform * point); points.emplace_back(transform * point);
} }
return result; return PointCloud(points);
} }
TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,

33
cartographer/sensor/point_cloud.h
View File

@ -28,9 +28,34 @@
namespace cartographer { namespace cartographer {
namespace sensor { namespace sensor {
// Stores 3D positions of points. // Stores 3D positions of points together with some additional data, e.g.
// For 2D points, the third entry is 0.f. // intensities.
using PointCloud = std::vector<RangefinderPoint>; 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 // 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 // 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). // third entry is 0.f (and the fourth entry is time).
using TimedPointCloud = std::vector<TimedRangefinderPoint>; using TimedPointCloud = std::vector<TimedRangefinderPoint>;
// TODO(wohe): Retained for cartographer_ros. To be removed once it is no
// longer used there.
struct PointCloudWithIntensities { struct PointCloudWithIntensities {
TimedPointCloud points; TimedPointCloud points;
std::vector<float> intensities; std::vector<float> intensities;

4
cartographer/sensor/point_cloud_test.cc
View File

@ -27,8 +27,8 @@ namespace {
TEST(PointCloudTest, TransformPointCloud) { TEST(PointCloudTest, TransformPointCloud) {
PointCloud point_cloud; PointCloud point_cloud;
point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.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}}); point_cloud.push_back({{Eigen::Vector3f{3.5f, 0.5f, 42.f}}});
const PointCloud transformed_point_cloud = TransformPointCloud( const PointCloud transformed_point_cloud = TransformPointCloud(
point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6);

7
cartographer/sensor/range_data.cc
View File

@ -53,7 +53,7 @@ proto::RangeData ToProto(const RangeData& range_data) {
} }
RangeData FromProto(const proto::RangeData& proto) { RangeData FromProto(const proto::RangeData& proto) {
PointCloud returns; std::vector<RangefinderPoint> returns;
if (proto.returns_size() > 0) { if (proto.returns_size() > 0) {
returns.reserve(proto.returns().size()); returns.reserve(proto.returns().size());
for (const auto& point_proto : proto.returns()) { for (const auto& point_proto : proto.returns()) {
@ -65,7 +65,7 @@ RangeData FromProto(const proto::RangeData& proto) {
returns.push_back({transform::ToEigen(point_proto)}); returns.push_back({transform::ToEigen(point_proto)});
} }
} }
PointCloud misses; std::vector<RangefinderPoint> misses;
if (proto.misses_size() > 0) { if (proto.misses_size() > 0) {
misses.reserve(proto.misses().size()); misses.reserve(proto.misses().size());
for (const auto& point_proto : proto.misses()) { for (const auto& point_proto : proto.misses()) {
@ -77,7 +77,8 @@ RangeData FromProto(const proto::RangeData& proto) {
misses.push_back({transform::ToEigen(point_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 } // namespace sensor