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

Keep intensities in the voxel filter. (#1764) 

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-19 12:14:50 +02:00 committed by GitHub
parent 952f59d499
commit d3473fca4a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 54 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -86,9 +86,9 @@ VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point,
}
template <class T, class PointFunction>
std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
const float resolution,
PointFunction&& point_function) {
std::vector<bool> RandomizedVoxelFilterIndices(
const std::vector<T>& point_cloud, const float resolution,
PointFunction&& point_function) {
// According to https://en.wikipedia.org/wiki/Reservoir_sampling
std::minstd_rand0 generator;
absl::flat_hash_map<VoxelKeyType, std::pair<int, int>>
@ -110,6 +110,15 @@ std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
for (const auto& voxel_and_index : voxel_count_and_point_index) {
points_used[voxel_and_index.second.second] = true;
}
return points_used;
}
template <class T, class PointFunction>
std::vector<T> RandomizedVoxelFilter(const std::vector<T>& point_cloud,
const float resolution,
PointFunction&& point_function) {
const std::vector<bool> points_used =
RandomizedVoxelFilterIndices(point_cloud, resolution, point_function);
std::vector<T> results;
for (size_t i = 0; i < point_cloud.size(); i++) {
@ -130,7 +139,25 @@ std::vector<RangefinderPoint> VoxelFilter(
}
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
return PointCloud(VoxelFilter(point_cloud.points(), resolution));
const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
point_cloud.points(), resolution,
[](const RangefinderPoint& point) { return point.position; });
std::vector<RangefinderPoint> filtered_points;
for (size_t i = 0; i < point_cloud.size(); i++) {
if (points_used[i]) {
filtered_points.push_back(point_cloud[i]);
}
}
std::vector<float> filtered_intensities;
CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size());
for (size_t i = 0; i < point_cloud.intensities().size(); i++) {
if (points_used[i]) {
filtered_intensities.push_back(point_cloud.intensities()[i]);
}
}
return PointCloud(std::move(filtered_points),
std::move(filtered_intensities));
}
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,

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

@ -25,6 +25,7 @@ namespace sensor {
namespace {
using ::testing::Contains;
using ::testing::IsEmpty;
TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
const PointCloud point_cloud({{{0.f, 0.f, 0.f}},
@ -33,11 +34,32 @@ TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
{{0.f, 0.f, 0.1f}}});
const PointCloud result = VoxelFilter(point_cloud, 0.3f);
ASSERT_EQ(result.size(), 2);
EXPECT_THAT(result.intensities(), IsEmpty());
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, CorrectIntensities) {
std::vector<RangefinderPoint> points;
std::vector<float> intensities;
for (int i = 0; i < 100; ++i) {
const float value = 0.1f * i;
// We add points with intensity equal to the z coordinate, so we can later
// verify that the resulting intensities are corresponding to the filtered
// points.
points.push_back({{-100.f, 0.3f, value}});
intensities.push_back(value);
}
const PointCloud point_cloud(points, intensities);
const PointCloud result = VoxelFilter(point_cloud, 0.3f);
ASSERT_EQ(result.intensities().size(), result.points().size());
for (size_t i = 0; i < result.size(); ++i) {
ASSERT_NEAR(result[i].position.z(), result.intensities()[i], 1e-6);
}
}
TEST(VoxelFilterTest, HandlesLargeCoordinates) {
const PointCloud point_cloud({{{100000.f, 0.f, 0.f}},
{{100000.001f, -0.0001f, 0.0001f}},
@ -45,6 +67,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.intensities(), IsEmpty());
EXPECT_THAT(result.points(), Contains(point_cloud[3]));
}