Removed redundant 2d voxel filtering (#1243)

Removed redundant adaptive voxel fitering in trajectory builder 2d.

Adaptive voxel filtering of the lidar point cloud was performed in InsertIntoSubmap and ScanMatch. Both methods are called from AddAccumulatedRangeData. Now, adaptive voxel filtering is done only once in AddAccumulatedRangeData and the filtered point cloud is then forwarded to InsertIntoSubmap and ScanMatch.

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -62,19 +62,13 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
 
 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
     const common::Time time, const transform::Rigid2d& pose_prediction,
-    const sensor::RangeData& gravity_aligned_range_data) {
+    const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
   std::shared_ptr<const Submap2D> matching_submap =
       active_submaps_.submaps().front();
   // The online correlative scan matcher will refine the initial estimate for
   // the Ceres scan matcher.
   transform::Rigid2d initial_ceres_pose = pose_prediction;
-  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
+
-      options_.adaptive_voxel_filter_options());
-  const sensor::PointCloud filtered_gravity_aligned_point_cloud =
-      adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
-  if (filtered_gravity_aligned_point_cloud.empty()) {
-    return nullptr;
-  }
   if (options_.use_online_correlative_scan_matching()) {
     CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
              proto::GridOptions2D_GridType_PROBABILITY_GRID);
@@ -222,9 +216,16 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
   const transform::Rigid2d pose_prediction = transform::Project2D(
       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);
+  if (filtered_gravity_aligned_point_cloud.empty()) {
+    return nullptr;
+  }
+
   // local map frame <- gravity-aligned frame
   std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
-      ScanMatch(time, pose_prediction, gravity_aligned_range_data);
+      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
   if (pose_estimate_2d == nullptr) {
     LOG(WARNING) << "Scan matching failed.";
     return nullptr;
@@ -236,8 +237,8 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
   sensor::RangeData range_data_in_local =
       TransformRangeData(gravity_aligned_range_data,
                          transform::Embed3D(pose_estimate_2d->cast<float>()));
-  std::unique_ptr<InsertionResult> insertion_result =
+  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
-      InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
+      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
       pose_estimate, gravity_alignment.rotation());
   auto duration = std::chrono::steady_clock::now() - accumulation_started_;
   kLocalSlamLatencyMetric->Set(
@@ -251,7 +252,7 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
 std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
 LocalTrajectoryBuilder2D::InsertIntoSubmap(
     const common::Time time, const sensor::RangeData& range_data_in_local,
-    const sensor::RangeData& gravity_aligned_range_data,
+    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
     const transform::Rigid3d& pose_estimate,
     const Eigen::Quaterniond& gravity_alignment) {
   if (motion_filter_.IsSimilar(time, pose_estimate)) {
@@ -266,11 +267,6 @@ LocalTrajectoryBuilder2D::InsertIntoSubmap(
   }
   active_submaps_.InsertRangeData(range_data_in_local);
 
-  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
-      options_.loop_closure_adaptive_voxel_filter_options());
-  const sensor::PointCloud filtered_gravity_aligned_point_cloud =
-      adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
-
   return common::make_unique<InsertionResult>(InsertionResult{
       std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
           time,

cartographer/mapping/internal/2d/local_trajectory_builder_2d.h

@@ -83,18 +83,17 @@ class LocalTrajectoryBuilder2D {
   sensor::RangeData TransformToGravityAlignedFrameAndFilter(
       const transform::Rigid3f& transform_to_gravity_aligned_frame,
       const sensor::RangeData& range_data) const;
-
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       common::Time time, const sensor::RangeData& range_data_in_local,
-      const sensor::RangeData& gravity_aligned_range_data,
+      const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
       const transform::Rigid3d& pose_estimate,
       const Eigen::Quaterniond& gravity_alignment);
 
-  // Scan matches 'gravity_aligned_range_data' and returns the observed pose,
+  // Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
-  // or nullptr on failure.
+  // observed pose, or nullptr on failure.
   std::unique_ptr<transform::Rigid2d> ScanMatch(
       common::Time time, const transform::Rigid2d& pose_prediction,
-      const sensor::RangeData& gravity_aligned_range_data);
+      const sensor::PointCloud& filtered_gravity_aligned_point_cloud);
 
   // Lazily constructs a PoseExtrapolator.
   void InitializeExtrapolator(common::Time time);