From b9015f33a775bfbe61c695737a3eb73dd9723b32 Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 16 Nov 2017 13:35:44 +0100 Subject: [PATCH] Filter early in AddRangeData. (#681) Filter earlier in 3D. Combine two transforms to one in 2D. --- .../mapping_2d/local_trajectory_builder.cc | 31 ++++++++++--------- .../mapping_2d/local_trajectory_builder.h | 7 +++-- .../mapping_3d/local_trajectory_builder.cc | 18 +++++------ .../mapping_3d/local_trajectory_builder.h | 3 +- 4 files changed, 32 insertions(+), 27 deletions(-) diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 5541a78..a54a44a 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -36,12 +36,14 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder( LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} -sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData( - const transform::Rigid3f& gravity_alignment, +sensor::RangeData +LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter( + const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const { - const sensor::RangeData cropped = sensor::CropRangeData( - sensor::TransformRangeData(range_data, gravity_alignment), - options_.min_z(), options_.max_z()); + const sensor::RangeData cropped = + sensor::CropRangeData(sensor::TransformRangeData( + range_data, transform_to_gravity_aligned_frame), + options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()), @@ -125,22 +127,23 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, if (num_accumulated_ >= options_.scans_per_accumulation()) { num_accumulated_ = 0; + const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( + extrapolator_->EstimateGravityOrientation(time)); return AddAccumulatedRangeData( - time, sensor::TransformRangeData(accumulated_range_data_, - tracking_delta.inverse())); + time, + TransformToGravityAlignedFrameAndFilter( + gravity_alignment.cast() * tracking_delta.inverse(), + accumulated_range_data_), + gravity_alignment); } return nullptr; } std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( - const common::Time time, const sensor::RangeData& range_data) { - // Transforms 'range_data' from the tracking frame into a frame where gravity - // direction is approximately +z. - const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( - extrapolator_->EstimateGravityOrientation(time)); - const sensor::RangeData gravity_aligned_range_data = - TransformAndFilterRangeData(gravity_alignment.cast(), range_data); + const common::Time time, + const sensor::RangeData& gravity_aligned_range_data, + const transform::Rigid3d& gravity_alignment) { if (gravity_aligned_range_data.returns.empty()) { LOG(WARNING) << "Dropped empty horizontal range data."; return nullptr; diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index f0dade0..c2a10ca 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -72,9 +72,10 @@ class LocalTrajectoryBuilder { private: std::unique_ptr AddAccumulatedRangeData( - common::Time time, const sensor::RangeData& range_data); - sensor::RangeData TransformAndFilterRangeData( - const transform::Rigid3f& gravity_alignment, + common::Time time, const sensor::RangeData& gravity_aligned_range_data, + const transform::Rigid3d& gravity_alignment); + sensor::RangeData TransformToGravityAlignedFrameAndFilter( + const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const; std::unique_ptr InsertIntoSubmap( diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 5981882..aae808d 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -100,8 +100,14 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, if (num_accumulated_ >= options_.scans_per_accumulation()) { 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())}; return AddAccumulatedRangeData( - time, sensor::TransformRangeData(accumulated_range_data_, + time, sensor::TransformRangeData(filtered_range_data, tracking_delta.inverse())); } return nullptr; @@ -109,14 +115,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( - const common::Time time, const sensor::RangeData& range_data_in_tracking) { - const sensor::RangeData filtered_range_data_in_tracking = { - range_data_in_tracking.origin, - sensor::VoxelFiltered(range_data_in_tracking.returns, - options_.voxel_filter_size()), - sensor::VoxelFiltered(range_data_in_tracking.misses, - options_.voxel_filter_size())}; - + const common::Time time, + const sensor::RangeData& filtered_range_data_in_tracking) { if (filtered_range_data_in_tracking.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index e922e6b..6ee3041 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -69,7 +69,8 @@ class LocalTrajectoryBuilder { private: std::unique_ptr AddAccumulatedRangeData( - common::Time time, const sensor::RangeData& range_data_in_tracking); + common::Time time, + const sensor::RangeData& filtered_range_data_in_tracking); std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& filtered_range_data_in_local,