Check if filtered point clouds are empty. (#664)

The point cloud will be filtered by MaxRangeFilter before doing scan matching. For some special cases, the filtered point cloud maybe empty, it will cause crash:
real_time_correlative_scan_matcher.cc:159 Check failed: candidate.score > 0.f (nan vs. 0) 
This PR fixed this bug, check the point cloud size before scan matching, return false for empty point cloud.

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -48,10 +48,9 @@ sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
       sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())};
 }
 
-void LocalTrajectoryBuilder::ScanMatch(
+std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
     const common::Time time, const transform::Rigid2d& pose_prediction,
-    const sensor::RangeData& gravity_aligned_range_data,
+    const sensor::RangeData& gravity_aligned_range_data) {
-    transform::Rigid2d* const pose_observation) {
   std::shared_ptr<const Submap> matching_submap =
       active_submaps_.submaps().front();
   // The online correlative scan matcher will refine the initial estimate for
@@ -61,16 +60,21 @@ void LocalTrajectoryBuilder::ScanMatch(
       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()) {
     real_time_correlative_scan_matcher_.Match(
         pose_prediction, filtered_gravity_aligned_point_cloud,
         matching_submap->probability_grid(), &initial_ceres_pose);
   }
 
+  auto pose_observation = common::make_unique<transform::Rigid2d>();
   ceres::Solver::Summary summary;
   ceres_scan_matcher_.Match(
       pose_prediction, initial_ceres_pose, filtered_gravity_aligned_point_cloud,
-      matching_submap->probability_grid(), pose_observation, &summary);
+      matching_submap->probability_grid(), pose_observation.get(), &summary);
+  return pose_observation;
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
@@ -149,16 +153,19 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
 
   // local map frame <- gravity-aligned frame
-  transform::Rigid2d pose_estimate_2d;
+  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
-  ScanMatch(time, pose_prediction, gravity_aligned_range_data,
+      ScanMatch(time, pose_prediction, gravity_aligned_range_data);
-            &pose_estimate_2d);
+  if (pose_estimate_2d == nullptr) {
+    LOG(WARNING) << "Scan matching failed.";
+    return nullptr;
+  }
   const transform::Rigid3d pose_estimate =
-      transform::Embed3D(pose_estimate_2d) * gravity_alignment;
+      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
   extrapolator_->AddPose(time, pose_estimate);
 
   sensor::RangeData range_data_in_local =
       TransformRangeData(gravity_aligned_range_data,
-                         transform::Embed3D(pose_estimate_2d.cast<float>()));
+                         transform::Embed3D(pose_estimate_2d->cast<float>()));
   last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
   std::unique_ptr<InsertionResult> insertion_result =
       InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,

cartographer/mapping_2d/local_trajectory_builder.h

@@ -83,11 +83,11 @@ class LocalTrajectoryBuilder {
       const transform::Rigid3d& pose_estimate,
       const Eigen::Quaterniond& gravity_alignment);
 
-  // Scan matches 'gravity_aligned_range_data' and fill in the
+  // Scan matches 'gravity_aligned_range_data' and returns the observed pose,
-  // 'pose_observation' with the result.
+  // or nullptr on failure.
-  void ScanMatch(common::Time time, const transform::Rigid2d& pose_prediction,
+  std::unique_ptr<transform::Rigid2d> ScanMatch(
-                 const sensor::RangeData& gravity_aligned_range_data,
+      common::Time time, const transform::Rigid2d& pose_prediction,
-                 transform::Rigid2d* pose_observation);
+      const sensor::RangeData& gravity_aligned_range_data);
 
   // Lazily constructs a PoseExtrapolator.
   void InitializeExtrapolator(common::Time time);

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -133,6 +133,10 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       options_.high_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud high_resolution_point_cloud_in_tracking =
       adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
+  if (high_resolution_point_cloud_in_tracking.empty()) {
+    LOG(WARNING) << "Dropped empty high resolution point cloud data.";
+    return nullptr;
+  }
   if (options_.use_online_correlative_scan_matching()) {
     // We take a copy since we use 'initial_ceres_pose' as an output argument.
     const transform::Rigid3d initial_pose = initial_ceres_pose;
@@ -149,6 +153,10 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   const sensor::PointCloud low_resolution_point_cloud_in_tracking =
       low_resolution_adaptive_voxel_filter.Filter(
           filtered_range_data_in_tracking.returns);
+  if (low_resolution_point_cloud_in_tracking.empty()) {
+    LOG(WARNING) << "Dropped empty low resolution point cloud data.";
+    return nullptr;
+  }
   ceres_scan_matcher_->Match(
       matching_submap->local_pose().inverse() * pose_prediction,
       initial_ceres_pose,