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

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

Adapts transforms/filters to use this new method.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-16 10:52:28 +02:00 committed by GitHub
parent 81d34ef185
commit 952f59d499
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 78 additions and 22 deletions

View File

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

View File

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

View File

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

View File

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

View File

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