Filter early in AddRangeData. (#681)

Filter earlier in 3D.
Combine two transforms to one in 2D.

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -36,12 +36,14 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
-sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
+sensor::RangeData
-    const transform::Rigid3f& gravity_alignment,
+LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
+    const transform::Rigid3f& transform_to_gravity_aligned_frame,
     const sensor::RangeData& range_data) const {
-  const sensor::RangeData cropped = sensor::CropRangeData(
+  const sensor::RangeData cropped =
-      sensor::TransformRangeData(range_data, gravity_alignment),
+      sensor::CropRangeData(sensor::TransformRangeData(
-      options_.min_z(), options_.max_z());
+                                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_,
+        time,
-                                         tracking_delta.inverse()));
+        TransformToGravityAlignedFrameAndFilter(
+            gravity_alignment.cast<float>() * tracking_delta.inverse(),
+            accumulated_range_data_),
+        gravity_alignment);
   }
   return nullptr;
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
-    const common::Time time, const sensor::RangeData& range_data) {
+    const common::Time time,
-  // Transforms 'range_data' from the tracking frame into a frame where gravity
+    const sensor::RangeData& gravity_aligned_range_data,
-  // direction is approximately +z.
+    const transform::Rigid3d& gravity_alignment) {
-  const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
-      extrapolator_->EstimateGravityOrientation(time));
-  const sensor::RangeData gravity_aligned_range_data =
-      TransformAndFilterRangeData(gravity_alignment.cast<float>(), range_data);
   if (gravity_aligned_range_data.returns.empty()) {
     LOG(WARNING) << "Dropped empty horizontal range data.";
     return nullptr;

cartographer/mapping_2d/local_trajectory_builder.h

@@ -72,9 +72,10 @@ class LocalTrajectoryBuilder {
 
  private:
   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
-      common::Time time, const sensor::RangeData& range_data);
+      common::Time time, const sensor::RangeData& gravity_aligned_range_data,
-  sensor::RangeData TransformAndFilterRangeData(
+      const transform::Rigid3d& gravity_alignment);
-      const transform::Rigid3f& gravity_alignment,
+  sensor::RangeData TransformToGravityAlignedFrameAndFilter(
+      const transform::Rigid3f& transform_to_gravity_aligned_frame,
       const sensor::RangeData& range_data) const;
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(

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::MatchingResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
-    const common::Time time, const sensor::RangeData& range_data_in_tracking) {
+    const common::Time time,
-  const sensor::RangeData filtered_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())};
-
   if (filtered_range_data_in_tracking.returns.empty()) {
     LOG(WARNING) << "Dropped empty range data.";
     return nullptr;

cartographer/mapping_3d/local_trajectory_builder.h

@@ -69,7 +69,8 @@ class LocalTrajectoryBuilder {
 
  private:
   std::unique_ptr<MatchingResult> 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<InsertionResult> InsertIntoSubmap(
       common::Time time, const sensor::RangeData& filtered_range_data_in_local,