:optimal

include/robot_costmap_2d/voxel_layer.h

@@ -99,6 +99,12 @@ private:
                             double* min_x, double* min_y, double* max_x, double* max_y);
   bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
                        double& depth_m, bool& is_valid) const;
+  /// True when the invalid pixel at (u, v) is an isolated hole surrounded by
+  /// valid returns (a flying pixel, safe to clear), false when it belongs to a
+  /// structurally invalid region such as the stereo no-disparity strip on the
+  /// left image edge (no free-space evidence, must be left untouched).
+  bool invalidPixelIsIsolated(const robot_sensor_msgs::Image& depth,
+                              unsigned int u, unsigned int v) const;
   void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
                            double fx, double fy, double cx, double cy);
   bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
@@ -125,7 +131,8 @@ private:
     double min_band_dist = -1.0;  ///< horizontal distance of nearest in-band return; < 0 = none
     double azimuth = 0.0;         ///< beam direction in the global frame
     double best_row_delta = std::numeric_limits<double>::infinity();
-    bool has_ray = false;         ///< column had at least one readable pixel
+    bool has_ray = false;            ///< column had at least one readable pixel
+    bool has_valid_return = false;   ///< column had at least one valid depth measurement
   };
   std::vector<DepthColumnStat> depth_column_stats_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;

plugins/voxel_layer.cpp

@@ -57,6 +57,19 @@ using robot_costmap_2d::Observation;
 
 namespace robot_costmap_2d
 {
+namespace
+{
+// Isolated-hole gate for invalid depth pixels. An invalid pixel may only clear
+// to max_range when at least kInvalidClearMinValidNeighbors of the 8 neighbours
+// sampled at kInvalidClearNeighborRadius px are valid. This separates small
+// flying-pixel / speckle holes (surrounded by valid data -> genuinely free ->
+// clear) from structurally invalid regions such as the stereo no-disparity
+// strip on the left image edge (no free-space evidence -> unknown -> leave
+// untouched), so obstacles leaving the FOV through that strip during rotation
+// are not erased.
+constexpr unsigned int kInvalidClearNeighborRadius = 3;      // [px]
+constexpr unsigned int kInvalidClearMinValidNeighbors = 6;   // of 8 sampled
+}  // namespace
 
 void VoxelLayer::onInitialize()
 {
@@ -456,6 +469,36 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned
   return false;
 }
 
+bool VoxelLayer::invalidPixelIsIsolated(const robot_sensor_msgs::Image& depth,
+                                        unsigned int u, unsigned int v) const
+{
+  // Sample 8 neighbours at a small radius. An isolated invalid pixel (flying
+  // pixel / speckle hole) is ringed by valid returns; a structural invalid
+  // block (stereo no-disparity strip, wide drop-outs) is not. Neighbours off
+  // the image border count as not-valid, so pixels hugging the invalid strip /
+  // frame edge stay classified as structural.
+  const int r = static_cast<int>(kInvalidClearNeighborRadius);
+  const int offsets[8][2] = {
+      {-r, 0}, {r, 0}, {0, -r}, {0, r}, {-r, -r}, {r, r}, {-r, r}, {r, -r}};
+  unsigned int valid_count = 0;
+  for (const auto& off : offsets)
+  {
+    const long nu = static_cast<long>(u) + off[0];
+    const long nv = static_cast<long>(v) + off[1];
+    if (nu < 0 || nv < 0)
+      continue;
+    double neighbor_depth = 0.0;
+    bool neighbor_valid = false;
+    if (readDepthMeters(depth, static_cast<unsigned int>(nu), static_cast<unsigned int>(nv),
+                        neighbor_depth, neighbor_valid) &&
+        neighbor_valid)
+    {
+      ++valid_count;
+    }
+  }
+  return valid_count >= kInvalidClearMinValidNeighbors;
+}
+
 void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
                                      unsigned int pixel_step, double fx, double fy,
                                      double cx, double cy)
@@ -771,6 +814,7 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
 
         if (valid)
         {
+          stat.has_valid_return = true;
           const double pz = oz + global_ray.z * euclid_range;
           if (pz >= band_min_h && pz <= band_max_h)
           {
@@ -785,6 +829,14 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
       }
     }
 
+    // Structurally-invalid depth (e.g. the stereo no-disparity strip on the
+    // left image edge) carries no evidence that the ray is free. Only let an
+    // invalid pixel clear to max_range when it is an isolated hole ringed by
+    // valid returns (a flying pixel); otherwise skip it so obstacles leaving
+    // the FOV through that strip during rotation are not erased.
+    if (!valid && !invalidPixelIsIsolated(depth, local_ray.u, local_ray.v))
+      continue;
+
     double ray_len = max_range;
     if (valid && euclid_range < max_range)
       ray_len = std::max(0.0, euclid_range - skip_dist);
@@ -891,6 +943,13 @@ bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
     if (!stat.has_ray)
       continue;
 
+    // A column with no in-band return only certifies free space when it
+    // actually measured something (floor / far surface). A column made only of
+    // invalid pixels (the stereo no-disparity strip) has no free-space
+    // evidence, so do not wipe the whole column out to max_range.
+    if (stat.min_band_dist < 0.0 && !stat.has_valid_return)
+      continue;
+
     double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;
     end_dist = std::min(std::min(end_dist, max_range), far_distance);
     if (end_dist <= start_dist)