:optimal 27/7 12:11

include/robot_costmap_2d/observation.h

@@ -70,6 +70,13 @@ struct DepthFrustumConfig
   /// Full columns are only cleared beyond this distance [m]. Negative:
   /// derive each frame from the camera intrinsics and mounting pose.
   double column_cover_distance = -1.0;
+  /// Depth-image columns at the LEFT edge excluded from clearing [px]. Covers
+  /// the stereo no-disparity strip that is permanently invalid there: those
+  /// pixels carry no free-space evidence, so clearing through them erases
+  /// obstacles that rotate out of the FOV on that side. Set to the measured
+  /// width of the black strip in the raw depth image (a few px margin). 0
+  /// disables. Invalid pixels ELSEWHERE still clear (needed for ghost removal).
+  unsigned int clear_left_border_px = 0;
 };
 
 class DepthCameraObservation

include/robot_costmap_2d/voxel_layer.h

@@ -99,12 +99,6 @@ 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);
@@ -132,7 +126,6 @@ private:
     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_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/obstacle_layer.cpp

@@ -168,6 +168,9 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
             loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance);
         frustum_config.column_cover_distance =
             loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance);
+        int frustum_clear_left_border =
+            loadParam(layer[source], "frustum_clear_left_border_px",
+                      loadParam(layer, "frustum_clear_left_border_px", 0));
 
         if (priv_nh.hasParam("topic"))
           priv_nh.getParam("topic", topic);
@@ -212,10 +215,14 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
           priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance);
         if (priv_nh.hasParam("column_cover_distance"))
           priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
+        if (priv_nh.hasParam("frustum_clear_left_border_px"))
+          priv_nh.getParam("frustum_clear_left_border_px", frustum_clear_left_border);
         if (priv_nh.hasParam("frustum_depth_camera_topic"))
           priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
 
         frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
+        frustum_config.clear_left_border_px =
+            static_cast<unsigned int>(std::max(0, frustum_clear_left_border));
 
         robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, "
                         "skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, "

plugins/voxel_layer.cpp

@@ -57,19 +57,6 @@ 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()
 {
@@ -469,36 +456,6 @@ 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)
@@ -782,6 +739,14 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
 
   for (const DepthRay& local_ray : depth_ray_cache_)
   {
+    // Skip the left-edge stereo no-disparity strip: those columns are
+    // permanently invalid, so clearing through them erases obstacles rotating
+    // out of the FOV on that side. Marking has no data there either, so nothing
+    // is lost. Invalid pixels ELSEWHERE still clear to max_range (ghost removal
+    // when an obstacle leaves and only far / open space remains behind it).
+    if (local_ray.u < frustum.clear_left_border_px)
+      continue;
+
     double depth_m = 0.0;
     bool valid = false;
     if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
@@ -814,7 +779,6 @@ 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)
           {
@@ -829,14 +793,6 @@ 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);
@@ -943,13 +899,6 @@ 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)