otimal

include/robot_costmap_2d/voxel_layer.h

@@ -51,6 +51,9 @@
 #include <robot_costmap_2d/obstacle_layer.h>
 #include <robot_voxel_grid/voxel_grid.h>
 
+#include <limits>
+#include <vector>
+
 namespace robot_costmap_2d
 {
 
@@ -102,11 +105,42 @@ private:
   bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
                      double raytrace_range, unsigned int cell_raytrace_range,
                      double* min_x, double* min_y, double* max_x, double* max_y);
+  bool clearDepthColumns(double ox, double oy, double cover_distance, double far_distance,
+                         double min_range, double max_range, double skip_dist,
+                         double* min_x, double* min_y, double* max_x, double* max_y);
+  bool clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const;
 
 
   bool publish_voxel_;
   robot_voxel_grid::VoxelGrid robot_voxel_grid_;
   double z_resolution_, origin_z_;
+  /// Clearing rays stop this far [m] before the measured surface.
+  /// Negative keeps the legacy 2 * resolution behaviour.
+  double frustum_skip_distance_ = -1.0;
+
+  /// Full-column clearing: per depth-image pixel column, the nearest return
+  /// inside the obstacle height band certifies "no obstacle in this direction
+  /// closer than d". Cells along that 2D beam get their whole voxel column
+  /// cleared, removing marked voxels the per-pixel 3D rays cannot reach
+  /// (above the vertical FOV at close range).
+  bool frustum_column_clearing_ = false;
+  /// Height band [m] used to detect the nearest in-band return. Points below
+  /// the band (floor) do not shorten the beam. max < 0: use max_obstacle_height.
+  double column_clear_min_height_ = 0.10;
+  double column_clear_max_height_ = -1.0;
+  /// Full columns are only cleared beyond this distance [m], where the
+  /// vertical FOV covers the whole height band. Negative: derive each frame
+  /// from the camera intrinsics and mounting pose.
+  double column_cover_distance_ = -1.0;
+
+  struct DepthColumnStat
+  {
+    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
+  };
+  std::vector<DepthColumnStat> depth_column_stats_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
   std::vector<unsigned char> rolling_costmap_scratch_;

plugins/voxel_layer.cpp

@@ -98,6 +98,11 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
     unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
     mark_threshold_ = loadParam(layer, "mark_threshold", 0);
     combination_method_ = loadParam(layer, "combination_method", 0.0);
+    frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
+    frustum_column_clearing_ = loadParam(layer, "frustum_column_clearing", false);
+    column_clear_min_height_ = loadParam(layer, "column_clear_min_height", 0.10);
+    column_clear_max_height_ = loadParam(layer, "column_clear_max_height", -1.0);
+    column_cover_distance_ = loadParam(layer, "column_cover_distance", -1.0);
 
     int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
     if (nh.hasParam("enabled"))
@@ -125,6 +130,26 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
     }
     if (nh.hasParam("combination_method"))
       nh.getParam("combination_method", combination_method_);
+    if (nh.hasParam("frustum_skip_distance"))
+      nh.getParam("frustum_skip_distance", frustum_skip_distance_);
+    if (nh.hasParam("frustum_column_clearing"))
+      nh.getParam("frustum_column_clearing", frustum_column_clearing_);
+    if (nh.hasParam("column_clear_min_height"))
+      nh.getParam("column_clear_min_height", column_clear_min_height_);
+    if (nh.hasParam("column_clear_max_height"))
+      nh.getParam("column_clear_max_height", column_clear_max_height_);
+    if (nh.hasParam("column_cover_distance"))
+      nh.getParam("column_cover_distance", column_cover_distance_);
+
+    robot::log_info("VoxelLayer frustum_skip_distance: %.3f m%s\n",
+                    frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_,
+                    frustum_skip_distance_ >= 0.0 ? "" : " (legacy 2 * resolution)");
+    robot::log_info("VoxelLayer frustum_column_clearing: %s, band: [%.2f, %.2f] m, cover_distance: %.2f m%s\n",
+                    frustum_column_clearing_ ? "true" : "false",
+                    column_clear_min_height_,
+                    column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_,
+                    column_cover_distance_,
+                    column_cover_distance_ >= 0.0 ? "" : " (auto from camera pose)");
 
     this->matchSize();
   } 
@@ -618,7 +643,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   const unsigned int step = std::max(1u, observation.pixel_step_);
   const double min_range = observation.min_range_;
   const double max_range = observation.max_range_;
-  const double skip_dist = 2.0 * resolution_;
+  const double skip_dist =
+      frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_;
   const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
   const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
   updateDepthRayCache(width, height, step, fx, fy, cx, cy);
@@ -647,6 +673,65 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   const unsigned int cell_raytrace_range = cellDistance(max_range);
   bool cleared_any = false;
 
+  // Column clearing: certify the beam length per pixel column and the distance
+  // window [cover, far] where the vertical FOV spans the whole height band.
+  // Outside that window a real obstacle could sit above/below the FOV, so only
+  // the per-pixel 3D rays may clear there.
+  const double band_min_h = column_clear_min_height_;
+  const double band_max_h =
+      column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_;
+  double cover_dist = column_cover_distance_;
+  double far_dist = std::numeric_limits<double>::infinity();
+  bool column_pass = frustum_column_clearing_ && band_max_h > band_min_h;
+
+  if (column_pass && cover_dist < 0.0)
+  {
+    const double up_half = std::atan2(cy, fy);
+    const double down_half = std::atan2(static_cast<double>(height) - 1.0 - cy, fy);
+    const double axis_elev = std::atan2(r22, std::hypot(r02, r12));
+    const double alpha_top = axis_elev + up_half;
+    const double alpha_bot = axis_elev - down_half;
+    const double band_top = band_max_h - oz;
+    const double band_bot = band_min_h - oz;
+    constexpr double kMinSlope = 1e-3;
+
+    cover_dist = 0.0;
+    if (band_top > 0.0)
+    {
+      if (alpha_top <= kMinSlope)
+        column_pass = false;  // camera can never look up to the band top
+      else
+        cover_dist = std::max(cover_dist, band_top / std::tan(alpha_top));
+    }
+    else if (alpha_top < -kMinSlope)
+    {
+      far_dist = std::min(far_dist, band_top / std::tan(alpha_top));
+    }
+    if (band_bot < 0.0)
+    {
+      if (alpha_bot >= -kMinSlope)
+        column_pass = false;  // camera can never look down to the band bottom
+      else
+        cover_dist = std::max(cover_dist, band_bot / std::tan(alpha_bot));
+    }
+    else if (alpha_bot > kMinSlope)
+    {
+      far_dist = std::min(far_dist, band_bot / std::tan(alpha_bot));
+    }
+
+    if (!column_pass)
+    {
+      robot::log_warning_throttle(
+          10.0, "VoxelLayer column clearing disabled: vertical FOV [%.1f, %.1f] deg at camera "
+          "height %.2f m never covers band [%.2f, %.2f] m\n",
+          alpha_bot * 180.0 / M_PI, alpha_top * 180.0 / M_PI, oz, band_min_h, band_max_h);
+    }
+  }
+
+  const unsigned int column_count = (width + step - 1) / step;
+  if (column_pass)
+    depth_column_stats_.assign(column_count, DepthColumnStat());
+
   for (const DepthRay& local_ray : depth_ray_cache_)
   {
     double depth_m = 0.0;
@@ -654,6 +739,47 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
       continue;
 
+    robot_geometry_msgs::Vector3 global_ray;
+    global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
+    global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
+    global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
+
+    if (column_pass)
+    {
+      const double horiz_norm = std::hypot(global_ray.x, global_ray.y);
+      const unsigned int col = local_ray.u / step;
+      if (horiz_norm > 1e-6 && col < column_count)
+      {
+        DepthColumnStat& stat = depth_column_stats_[col];
+        const double row_delta = std::fabs(static_cast<double>(local_ray.v) - cy);
+        if (stat.min_band_dist < 0.0 && row_delta < stat.best_row_delta)
+        {
+          // no in-band return yet: aim the beam along the ray nearest the
+          // principal row
+          stat.azimuth = std::atan2(global_ray.y, global_ray.x);
+          stat.best_row_delta = row_delta;
+        }
+        stat.has_ray = true;
+
+        if (valid)
+        {
+          // depth images store z-depth; local_ray.z is the unit ray's optical
+          // axis component, so depth / z is the Euclidean range
+          const double euclid_range = depth_m / local_ray.z;
+          const double pz = oz + global_ray.z * euclid_range;
+          if (pz >= band_min_h && pz <= band_max_h)
+          {
+            const double dist_h = horiz_norm * euclid_range;
+            if (stat.min_band_dist < 0.0 || dist_h < stat.min_band_dist)
+            {
+              stat.min_band_dist = dist_h;
+              stat.azimuth = std::atan2(global_ray.y, global_ray.x);
+            }
+          }
+        }
+      }
+    }
+
     double ray_len = max_range;
     if (valid && depth_m < max_range)
       ray_len = std::max(0.0, depth_m - skip_dist);
@@ -661,11 +787,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     if (ray_len <= min_range)
       continue;
 
-    robot_geometry_msgs::Vector3 global_ray;
-    global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
-    global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
-    global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
-
     const double sx = ox + global_ray.x * min_range;
     const double sy = oy + global_ray.y * min_range;
     const double sz = oz + global_ray.z * min_range;
@@ -677,6 +798,118 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
                                 min_x, min_y, max_x, max_y) || cleared_any;
   }
 
+  if (column_pass)
+  {
+    cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range,
+                                    skip_dist, min_x, min_y, max_x, max_y) ||
+                  cleared_any;
+  }
+
+  return cleared_any;
+}
+
+namespace
+{
+/// raytraceLine action: frees the 2D cell and wipes its whole voxel column.
+class ClearFullColumn
+{
+public:
+  ClearFullColumn(unsigned char* costmap, robot_voxel_grid::VoxelGrid& voxel_grid)
+    : costmap_(costmap), voxel_grid_(voxel_grid)
+  {
+  }
+
+  inline void operator()(unsigned int offset)
+  {
+    costmap_[offset] = FREE_SPACE;
+    voxel_grid_.clearVoxelColumn(offset);
+  }
+
+private:
+  unsigned char* costmap_;
+  robot_voxel_grid::VoxelGrid& voxel_grid_;
+};
+}  // namespace
+
+bool VoxelLayer::clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const
+{
+  // Liang-Barsky clip against the map interior; the half-resolution margin
+  // keeps clipped endpoints valid for worldToMap.
+  const double min_wx = origin_x_;
+  const double min_wy = origin_y_;
+  const double max_wx = origin_x_ + getSizeInMetersX() - 0.5 * resolution_;
+  const double max_wy = origin_y_ + getSizeInMetersY() - 0.5 * resolution_;
+  const double dx = ex - sx;
+  const double dy = ey - sy;
+  const double p[4] = {-dx, dx, -dy, dy};
+  const double q[4] = {sx - min_wx, max_wx - sx, sy - min_wy, max_wy - sy};
+
+  double t0 = 0.0;
+  double t1 = 1.0;
+  for (int i = 0; i < 4; ++i)
+  {
+    if (std::fabs(p[i]) < 1e-12)
+    {
+      if (q[i] < 0.0)
+        return false;
+      continue;
+    }
+    const double r = q[i] / p[i];
+    if (p[i] < 0.0)
+      t0 = std::max(t0, r);
+    else
+      t1 = std::min(t1, r);
+  }
+  if (t0 > t1)
+    return false;
+
+  const double bx = sx;
+  const double by = sy;
+  sx = bx + t0 * dx;
+  sy = by + t0 * dy;
+  ex = bx + t1 * dx;
+  ey = by + t1 * dy;
+  return true;
+}
+
+bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
+                                   double far_distance, double min_range, double max_range,
+                                   double skip_dist, double* min_x, double* min_y,
+                                   double* max_x, double* max_y)
+{
+  const double start_dist = std::max(cover_distance, min_range);
+  bool cleared_any = false;
+
+  for (const DepthColumnStat& stat : depth_column_stats_)
+  {
+    if (!stat.has_ray)
+      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)
+      continue;
+
+    const double cos_az = std::cos(stat.azimuth);
+    const double sin_az = std::sin(stat.azimuth);
+    double sx = ox + cos_az * start_dist;
+    double sy = oy + sin_az * start_dist;
+    double ex = ox + cos_az * end_dist;
+    double ey = oy + sin_az * end_dist;
+    if (!clipColumnSegment(sx, sy, ex, ey))
+      continue;
+
+    unsigned int sx_m, sy_m, ex_m, ey_m;
+    if (!worldToMap(sx, sy, sx_m, sy_m) || !worldToMap(ex, ey, ex_m, ey_m))
+      continue;
+
+    ClearFullColumn clearer(costmap_, robot_voxel_grid_);
+    raytraceLine(clearer, sx_m, sy_m, ex_m, ey_m);
+    touch(sx, sy, min_x, min_y, max_x, max_y);
+    touch(ex, ey, min_x, min_y, max_x, max_y);
+    cleared_any = true;
+  }
+
   return cleared_any;
 }