diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.cc b/cartographer/internal/mapping_2d/local_trajectory_builder.cc index 5b6fb95..50baa2b 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_2d/local_trajectory_builder.cc @@ -46,8 +46,8 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter( options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, - sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()), - sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())}; + sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns), + sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)}; } std::unique_ptr LocalTrajectoryBuilder::ScanMatch( diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder.cc index d0b7135..22ad471 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder.cc @@ -102,10 +102,10 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, num_accumulated_ = 0; const sensor::RangeData filtered_range_data = { accumulated_range_data_.origin, - sensor::VoxelFiltered(accumulated_range_data_.returns, - options_.voxel_filter_size()), - sensor::VoxelFiltered(accumulated_range_data_.misses, - options_.voxel_filter_size())}; + sensor::VoxelFilter(options_.voxel_filter_size()) + .Filter(accumulated_range_data_.returns), + sensor::VoxelFilter(options_.voxel_filter_size()) + .Filter(accumulated_range_data_.misses)}; return AddAccumulatedRangeData( time, sensor::TransformRangeData(filtered_range_data, tracking_delta.inverse())); diff --git a/cartographer/sensor/voxel_filter.cc b/cartographer/sensor/voxel_filter.cc index 9bb6d47..aad03d3 100644 --- a/cartographer/sensor/voxel_filter.cc +++ b/cartographer/sensor/voxel_filter.cc @@ -43,7 +43,7 @@ PointCloud AdaptivelyVoxelFiltered( // 'point_cloud' is already sparse enough. 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()) { // Filtering with 'max_length' resulted in a sufficiently dense point cloud. return result; @@ -54,14 +54,15 @@ 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 = VoxelFiltered(point_cloud, low_length); + result = VoxelFilter(low_length).Filter(point_cloud); 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 = VoxelFiltered(point_cloud, mid_length); + const PointCloud candidate = + VoxelFilter(mid_length).Filter(point_cloud); if (candidate.size() >= options.min_num_points()) { low_length = mid_length; result = candidate; @@ -75,27 +76,33 @@ PointCloud AdaptivelyVoxelFiltered( return result; } -} // namespace - -PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) { - VoxelFilter voxel_filter(size); - voxel_filter.InsertPointCloud(point_cloud); - return voxel_filter.point_cloud(); -} - -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)); +template +PointCloudType FilterPointCloudUsingVoxels( + const PointCloudType& point_cloud, + mapping_3d::HybridGridBase* voxels) { + PointCloudType results; + for (const auto& point : point_cloud) { + auto* const value = + voxels->mutable_value(voxels->GetCellIndex(point.template head<3>())); if (*value == 0) { - point_cloud_.push_back(point); + results.push_back(point); *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( common::LuaParameterDictionary* const parameter_dictionary) { diff --git a/cartographer/sensor/voxel_filter.h b/cartographer/sensor/voxel_filter.h index 5b6da59..8ecb23e 100644 --- a/cartographer/sensor/voxel_filter.h +++ b/cartographer/sensor/voxel_filter.h @@ -25,10 +25,6 @@ namespace cartographer { 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 // contains the first point that fell into it from any of the inserted point // clouds. @@ -40,15 +36,14 @@ class VoxelFilter { VoxelFilter(const VoxelFilter&) = delete; VoxelFilter& operator=(const VoxelFilter&) = delete; - // Inserts a point cloud into the voxel filter. - void InsertPointCloud(const PointCloud& point_cloud); + // Returns a voxel filtered copy of 'point_cloud'. + PointCloud Filter(const PointCloud& point_cloud); - // Returns the filtered point cloud representing the occupied voxels. - const PointCloud& point_cloud() const; + // Same for TimedPointCloud. + TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); private: mapping_3d::HybridGridBase voxels_; - PointCloud point_cloud_; }; proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( diff --git a/cartographer/sensor/voxel_filter_test.cc b/cartographer/sensor/voxel_filter_test.cc index ca95291..8187eea 100644 --- a/cartographer/sensor/voxel_filter_test.cc +++ b/cartographer/sensor/voxel_filter_test.cc @@ -31,7 +31,7 @@ TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { {0.1f, -0.1f, 0.1f}, {0.3f, -0.1f, 0.f}, {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]})); }