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

Random voxel filtering. (#1753) 

This leads to slightly slower performance but better quality.
Also replaces the VoxelFilter class by free standing functions.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-09-30 18:32:39 +02:00 committed by GitHub
parent 1ad6398bce
commit c84da8ec0f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 123 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -58,8 +58,8 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
options_.min_z(), options_.max_z());
return sensor::RangeData{
cropped.origin,
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
@ -226,8 +226,8 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
.Filter(gravity_aligned_range_data.returns);
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
options_.adaptive_voxel_filter_options());
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}

26
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -154,9 +154,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
accumulated_point_cloud_origin_data_.clear();
}
synchronized_data.ranges =
sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
.Filter(synchronized_data.ranges);
synchronized_data.ranges = sensor::VoxelFilter(
synchronized_data.ranges, 0.5f * options_.voxel_filter_size());
accumulated_point_cloud_origin_data_.emplace_back(
std::move(synchronized_data));
++num_accumulated_;
@ -237,10 +236,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
const auto voxel_filter_start = std::chrono::steady_clock::now();
const sensor::RangeData filtered_range_data = {
extrapolation_result.current_pose.translation().cast<float>(),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data.returns),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data.misses)};
sensor::VoxelFilter(accumulated_range_data.returns,
options_.voxel_filter_size()),
sensor::VoxelFilter(accumulated_range_data.misses,
options_.voxel_filter_size())};
const auto voxel_filter_stop = std::chrono::steady_clock::now();
const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
@ -274,19 +273,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
const auto scan_matcher_start = std::chrono::steady_clock::now();
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud high_resolution_point_cloud_in_tracking =
adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
sensor::AdaptiveVoxelFilter(
filtered_range_data_in_tracking.returns,
options_.high_resolution_adaptive_voxel_filter_options());
if (high_resolution_point_cloud_in_tracking.empty()) {
LOG(WARNING) << "Dropped empty high resolution point cloud data.";
return nullptr;
}
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
options_.low_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud low_resolution_point_cloud_in_tracking =
low_resolution_adaptive_voxel_filter.Filter(
filtered_range_data_in_tracking.returns);
sensor::AdaptiveVoxelFilter(
filtered_range_data_in_tracking.returns,
options_.low_resolution_adaptive_voxel_filter_options());
if (low_resolution_point_cloud_in_tracking.empty()) {
LOG(WARNING) << "Dropped empty low resolution point cloud data.";
return nullptr;

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

@ -17,7 +17,10 @@
#include "cartographer/sensor/internal/voxel_filter.h"
#include <cmath>
#include <random>
#include <utility>
#include "absl/container/flat_hash_map.h"
#include "cartographer/common/math.h"
namespace cartographer {
@ -43,7 +46,7 @@ PointCloud AdaptivelyVoxelFiltered(
// 'point_cloud' is already sparse enough.
return point_cloud;
}
PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
PointCloud result = VoxelFilter(point_cloud, options.max_length());
if (result.size() >= options.min_num_points()) {
// Filtering with 'max_length' resulted in a sufficiently dense point cloud.
return result;
@ -54,15 +57,14 @@ PointCloud AdaptivelyVoxelFiltered(
for (float high_length = options.max_length();
high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
float low_length = high_length / 2.f;
result = VoxelFilter(low_length).Filter(point_cloud);
result = VoxelFilter(point_cloud, low_length);
if (result.size() >= options.min_num_points()) {
// Binary search to find the right amount of filtering. 'low_length' gave
// a sufficiently dense 'result', 'high_length' did not. We stop when the
// edge length is at most 10% off.
while ((high_length - low_length) / low_length > 1e-1f) {
const float mid_length = (low_length + high_length) / 2.f;
const PointCloud candidate =
VoxelFilter(mid_length).Filter(point_cloud);
const PointCloud candidate = VoxelFilter(point_cloud, mid_length);
if (candidate.size() >= options.min_num_points()) {
low_length = mid_length;
result = candidate;
@ -76,58 +78,77 @@ PointCloud AdaptivelyVoxelFiltered(
return result;
}
using VoxelKeyType = uint64_t;
VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point,
const float resolution) {
const Eigen::Array3f index = point.array() / resolution;
const uint64_t x = common::RoundToInt(index.x());
const uint64_t y = common::RoundToInt(index.y());
const uint64_t z = common::RoundToInt(index.z());
return (x << 42) + (y << 21) + z;
}
template <class T, class PointFunction>
std::vector<T> RandomizedVoxelFilter(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>>
voxel_count_and_point_index;
for (size_t i = 0; i < point_cloud.size(); i++) {
auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex(
point_function(point_cloud[i]), resolution)];
voxel.first++;
if (voxel.first == 1) {
voxel.second = i;
} else {
std::uniform_int_distribution<> distribution(1, voxel.first);
if (distribution(generator) == voxel.first) {
voxel.second = i;
}
}
}
std::vector<bool> points_used(point_cloud.size(), false);
for (const auto& voxel_and_index : voxel_count_and_point_index) {
points_used[voxel_and_index.second.second] = true;
}
std::vector<T> results;
for (size_t i = 0; i < point_cloud.size(); i++) {
if (points_used[i]) {
results.push_back(point_cloud[i]);
}
}
return results;
}
} // namespace
PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
PointCloud results;
for (const RangefinderPoint& point : point_cloud) {
auto it_inserted =
voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
if (it_inserted.second) {
results.push_back(point);
}
}
return results;
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
return RandomizedVoxelFilter(
point_cloud, resolution,
[](const RangefinderPoint& point) { return point.position; });
}
TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
TimedPointCloud results;
for (const TimedRangefinderPoint& point : timed_point_cloud) {
auto it_inserted =
voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
if (it_inserted.second) {
results.push_back(point);
}
}
return results;
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
const float resolution) {
return RandomizedVoxelFilter(
timed_point_cloud, resolution,
[](const TimedRangefinderPoint& point) { return point.position; });
}
std::vector<TimedPointCloudOriginData::RangeMeasurement> VoxelFilter::Filter(
const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
range_measurements) {
std::vector<TimedPointCloudOriginData::RangeMeasurement> results;
for (const auto& range_measurement : range_measurements) {
auto it_inserted = voxel_set_.insert(
IndexToKey(GetCellIndex(range_measurement.point_time.position)));
if (it_inserted.second) {
results.push_back(range_measurement);
}
}
return results;
}
VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) {
KeyType k_0(static_cast<uint32>(index[0]));
KeyType k_1(static_cast<uint32>(index[1]));
KeyType k_2(static_cast<uint32>(index[2]));
return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2;
}
Eigen::Array3i VoxelFilter::GetCellIndex(const Eigen::Vector3f& point) const {
Eigen::Array3f index = point.array() / resolution_;
return Eigen::Array3i(common::RoundToInt(index.x()),
common::RoundToInt(index.y()),
common::RoundToInt(index.z()));
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
range_measurements,
const float resolution) {
return RandomizedVoxelFilter(
range_measurements, resolution,
[](const sensor::TimedPointCloudOriginData::RangeMeasurement&
range_measurement) {
return range_measurement.point_time.position;
});
}
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
@ -140,13 +161,11 @@ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
return options;
}
AdaptiveVoxelFilter::AdaptiveVoxelFilter(
const proto::AdaptiveVoxelFilterOptions& options)
: options_(options) {}
PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const {
PointCloud AdaptiveVoxelFilter(
const PointCloud& point_cloud,
const proto::AdaptiveVoxelFilterOptions& options) {
return AdaptivelyVoxelFiltered(
options_, FilterByMaxRange(point_cloud, options_.max_range()));
options, FilterByMaxRange(point_cloud, options.max_range()));
}
} // namespace sensor

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

@ -19,7 +19,6 @@
#include <bitset>
#include "absl/container/flat_hash_set.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h"
@ -28,56 +27,21 @@
namespace cartographer {
namespace sensor {
// Voxel filter for point clouds. For each voxel, the assembled point cloud
// contains the first point that fell into it from any of the inserted point
// clouds.
class VoxelFilter {
public:
// 'size' is the length of a voxel edge.
explicit VoxelFilter(float size) : resolution_(size) {}
VoxelFilter(const VoxelFilter&) = delete;
VoxelFilter& operator=(const VoxelFilter&) = delete;
// Returns a voxel filtered copy of 'point_cloud'.
PointCloud Filter(const PointCloud& point_cloud);
// Same for TimedPointCloud.
TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
// Same for RangeMeasurement.
std::vector<TimedPointCloudOriginData::RangeMeasurement> Filter(
const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
range_measurements);
private:
using KeyType = std::bitset<3 * 32>;
static KeyType IndexToKey(const Eigen::Array3i& index);
Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const;
float resolution_;
absl::flat_hash_set<KeyType> voxel_set_;
};
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution);
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
const float resolution);
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
range_measurements,
const float resolution);
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
common::LuaParameterDictionary* const parameter_dictionary);
class AdaptiveVoxelFilter {
public:
explicit AdaptiveVoxelFilter(
PointCloud AdaptiveVoxelFilter(
const PointCloud& point_cloud,
const proto::AdaptiveVoxelFilterOptions& options);
AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete;
AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete;
PointCloud Filter(const PointCloud& point_cloud) const;
private:
const proto::AdaptiveVoxelFilterOptions options_;
};
} // namespace sensor
} // namespace cartographer

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

@ -24,33 +24,38 @@ namespace cartographer {
namespace sensor {
namespace {
using ::testing::ContainerEq;
using ::testing::Contains;
TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}},
{Eigen::Vector3f{0.1f, -0.1f, 0.1f}},
{Eigen::Vector3f{0.3f, -0.1f, 0.f}},
{Eigen::Vector3f{0.f, 0.f, 0.1f}}};
EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud),
ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) {
const PointCloud point_cloud({{{0.f, 0.f, 0.f}},
{{0.1f, -0.1f, 0.1f}},
{{0.3f, -0.1f, 0.f}},
{{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]));
}
TEST(VoxelFilterTest, HandlesLargeCoordinates) {
PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}},
{Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}},
{Eigen::Vector3f{100000.003f, -0.0001f, 0.f}},
{Eigen::Vector3f{-200000.f, 0.f, 0.f}}};
EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud),
ContainerEq(PointCloud{point_cloud[0], point_cloud[3]}));
const PointCloud point_cloud({{{100000.f, 0.f, 0.f}},
{{100000.001f, -0.0001f, 0.0001f}},
{{100000.003f, -0.0001f, 0.f}},
{{-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]));
}
TEST(VoxelFilterTest, IgnoresTime) {
TimedPointCloud timed_point_cloud;
for (int i = 0; i < 100; ++i) {
timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i});
timed_point_cloud.push_back({{-100.f, 0.3f, 0.4f}, 1.f * i});
}
EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud),
ContainerEq(TimedPointCloud{timed_point_cloud[0]}));
const TimedPointCloud result = VoxelFilter(timed_point_cloud, 0.3f);
ASSERT_EQ(result.size(), 1);
EXPECT_THAT(timed_point_cloud, Contains(result[0]));
}
} // namespace