VoxelFilter for TimedPointCloud, Filter method. (#710)
This makes the VoxelFilter ready for TimedPointCloud and per-point unwarping.master
parent
c292d76f86
commit
1c7183d5c4
|
@ -46,8 +46,8 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
|
||||||
options_.min_z(), options_.max_z());
|
options_.min_z(), options_.max_z());
|
||||||
return sensor::RangeData{
|
return sensor::RangeData{
|
||||||
cropped.origin,
|
cropped.origin,
|
||||||
sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()),
|
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
|
||||||
sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())};
|
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
|
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
|
||||||
|
|
|
@ -102,10 +102,10 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||||
num_accumulated_ = 0;
|
num_accumulated_ = 0;
|
||||||
const sensor::RangeData filtered_range_data = {
|
const sensor::RangeData filtered_range_data = {
|
||||||
accumulated_range_data_.origin,
|
accumulated_range_data_.origin,
|
||||||
sensor::VoxelFiltered(accumulated_range_data_.returns,
|
sensor::VoxelFilter(options_.voxel_filter_size())
|
||||||
options_.voxel_filter_size()),
|
.Filter(accumulated_range_data_.returns),
|
||||||
sensor::VoxelFiltered(accumulated_range_data_.misses,
|
sensor::VoxelFilter(options_.voxel_filter_size())
|
||||||
options_.voxel_filter_size())};
|
.Filter(accumulated_range_data_.misses)};
|
||||||
return AddAccumulatedRangeData(
|
return AddAccumulatedRangeData(
|
||||||
time, sensor::TransformRangeData(filtered_range_data,
|
time, sensor::TransformRangeData(filtered_range_data,
|
||||||
tracking_delta.inverse()));
|
tracking_delta.inverse()));
|
||||||
|
|
|
@ -43,7 +43,7 @@ PointCloud AdaptivelyVoxelFiltered(
|
||||||
// 'point_cloud' is already sparse enough.
|
// 'point_cloud' is already sparse enough.
|
||||||
return point_cloud;
|
return point_cloud;
|
||||||
}
|
}
|
||||||
PointCloud result = VoxelFiltered(point_cloud, options.max_length());
|
PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
|
||||||
if (result.size() >= options.min_num_points()) {
|
if (result.size() >= options.min_num_points()) {
|
||||||
// Filtering with 'max_length' resulted in a sufficiently dense point cloud.
|
// Filtering with 'max_length' resulted in a sufficiently dense point cloud.
|
||||||
return result;
|
return result;
|
||||||
|
@ -54,14 +54,15 @@ PointCloud AdaptivelyVoxelFiltered(
|
||||||
for (float high_length = options.max_length();
|
for (float high_length = options.max_length();
|
||||||
high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
|
high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
|
||||||
float low_length = high_length / 2.f;
|
float low_length = high_length / 2.f;
|
||||||
result = VoxelFiltered(point_cloud, low_length);
|
result = VoxelFilter(low_length).Filter(point_cloud);
|
||||||
if (result.size() >= options.min_num_points()) {
|
if (result.size() >= options.min_num_points()) {
|
||||||
// Binary search to find the right amount of filtering. 'low_length' gave
|
// Binary search to find the right amount of filtering. 'low_length' gave
|
||||||
// a sufficiently dense 'result', 'high_length' did not. We stop when the
|
// a sufficiently dense 'result', 'high_length' did not. We stop when the
|
||||||
// edge length is at most 10% off.
|
// edge length is at most 10% off.
|
||||||
while ((high_length - low_length) / low_length > 1e-1f) {
|
while ((high_length - low_length) / low_length > 1e-1f) {
|
||||||
const float mid_length = (low_length + high_length) / 2.f;
|
const float mid_length = (low_length + high_length) / 2.f;
|
||||||
const PointCloud candidate = VoxelFiltered(point_cloud, mid_length);
|
const PointCloud candidate =
|
||||||
|
VoxelFilter(mid_length).Filter(point_cloud);
|
||||||
if (candidate.size() >= options.min_num_points()) {
|
if (candidate.size() >= options.min_num_points()) {
|
||||||
low_length = mid_length;
|
low_length = mid_length;
|
||||||
result = candidate;
|
result = candidate;
|
||||||
|
@ -75,27 +76,33 @@ PointCloud AdaptivelyVoxelFiltered(
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
template <typename PointCloudType>
|
||||||
|
PointCloudType FilterPointCloudUsingVoxels(
|
||||||
PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
|
const PointCloudType& point_cloud,
|
||||||
VoxelFilter voxel_filter(size);
|
mapping_3d::HybridGridBase<uint8>* voxels) {
|
||||||
voxel_filter.InsertPointCloud(point_cloud);
|
PointCloudType results;
|
||||||
return voxel_filter.point_cloud();
|
for (const auto& point : point_cloud) {
|
||||||
}
|
auto* const value =
|
||||||
|
voxels->mutable_value(voxels->GetCellIndex(point.template head<3>()));
|
||||||
VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
|
|
||||||
|
|
||||||
void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
|
|
||||||
for (const Eigen::Vector3f& point : point_cloud) {
|
|
||||||
auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point));
|
|
||||||
if (*value == 0) {
|
if (*value == 0) {
|
||||||
point_cloud_.push_back(point);
|
results.push_back(point);
|
||||||
*value = 1;
|
*value = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
return results;
|
||||||
}
|
}
|
||||||
|
|
||||||
const PointCloud& VoxelFilter::point_cloud() const { return point_cloud_; }
|
} // namespace
|
||||||
|
|
||||||
|
VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
|
||||||
|
|
||||||
|
PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
|
||||||
|
return FilterPointCloudUsingVoxels(point_cloud, &voxels_);
|
||||||
|
}
|
||||||
|
|
||||||
|
TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
|
||||||
|
return FilterPointCloudUsingVoxels(timed_point_cloud, &voxels_);
|
||||||
|
}
|
||||||
|
|
||||||
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
|
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
|
||||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||||
|
|
|
@ -25,10 +25,6 @@
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace sensor {
|
namespace sensor {
|
||||||
|
|
||||||
// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length
|
|
||||||
// a voxel edge.
|
|
||||||
PointCloud VoxelFiltered(const PointCloud& point_cloud, float size);
|
|
||||||
|
|
||||||
// Voxel filter for point clouds. For each voxel, the assembled point cloud
|
// 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
|
// contains the first point that fell into it from any of the inserted point
|
||||||
// clouds.
|
// clouds.
|
||||||
|
@ -40,15 +36,14 @@ class VoxelFilter {
|
||||||
VoxelFilter(const VoxelFilter&) = delete;
|
VoxelFilter(const VoxelFilter&) = delete;
|
||||||
VoxelFilter& operator=(const VoxelFilter&) = delete;
|
VoxelFilter& operator=(const VoxelFilter&) = delete;
|
||||||
|
|
||||||
// Inserts a point cloud into the voxel filter.
|
// Returns a voxel filtered copy of 'point_cloud'.
|
||||||
void InsertPointCloud(const PointCloud& point_cloud);
|
PointCloud Filter(const PointCloud& point_cloud);
|
||||||
|
|
||||||
// Returns the filtered point cloud representing the occupied voxels.
|
// Same for TimedPointCloud.
|
||||||
const PointCloud& point_cloud() const;
|
TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
mapping_3d::HybridGridBase<uint8> voxels_;
|
mapping_3d::HybridGridBase<uint8> voxels_;
|
||||||
PointCloud point_cloud_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
|
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
|
||||||
|
|
|
@ -31,7 +31,7 @@ TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
|
||||||
{0.1f, -0.1f, 0.1f},
|
{0.1f, -0.1f, 0.1f},
|
||||||
{0.3f, -0.1f, 0.f},
|
{0.3f, -0.1f, 0.f},
|
||||||
{0.f, 0.f, 0.1f}};
|
{0.f, 0.f, 0.1f}};
|
||||||
EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f),
|
EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud),
|
||||||
ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
|
ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue