optimal voxel layer

include/robot_costmap_2d/voxel_layer.h

@@ -97,20 +97,37 @@ private:
   bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
                        double& depth_m, bool& is_valid) const;
   void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
-                           double fx, double fy, double cx, double cy);
+                           double fx, double fy, double cx, double cy,
+                           unsigned int u_offset, unsigned int v_offset);
   bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
   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);
+  /// Frees LETHAL cells this layer marked that have not been re-marked within
+  /// obstacle_decay_time. Handles ghost cells frustum clearing can never
+  /// reach: cells that left the camera FOV, occlusion shadows, and cells
+  /// inside the skip band in front of a measured surface.
+  void decayStaleObstacles(double now_sec, double* min_x, double* min_y,
+                           double* max_x, double* max_y);
 
 
   bool publish_voxel_;
   robot_voxel_grid::VoxelGrid robot_voxel_grid_;
   double z_resolution_, origin_z_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
+  /// Seconds a marked cell survives without being re-observed before it is
+  /// freed. <= 0 disables decay (default). Only enable on camera-only
+  /// costmaps: decay clears obstacles the sensor cannot currently see.
+  double obstacle_decay_time_{0.0};
+  /// Clearing rays stop this far [m] before the measured surface. < 0 keeps
+  /// the legacy 2 * resolution behaviour.
+  double frustum_skip_distance_{-1.0};
+  /// Per-cell robot::Time seconds of the last marking; 0 = no valid stamp.
+  std::vector<double> cell_last_marked_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
   std::vector<unsigned char> rolling_costmap_scratch_;
   std::vector<unsigned int> rolling_voxel_scratch_;
+  std::vector<double> rolling_stamp_scratch_;
 
   struct DepthRay
   {
@@ -128,6 +145,14 @@ private:
   double cached_fy_ = 0.0;
   double cached_cx_ = 0.0;
   double cached_cy_ = 0.0;
+  unsigned int cached_u_offset_ = 0;
+  unsigned int cached_v_offset_ = 0;
+  /// Advances every frustum pass to dither the sampled pixel grid. A static
+  /// camera otherwise re-traces the same fixed pixel_step subgrid each frame,
+  /// so cells between adjacent rays (angular gap pixel_step / fx) are never
+  /// crossed and stay marked until the robot moves. Cycling the grid phase
+  /// sweeps every pixel over pixel_step^2 frames at unchanged per-frame cost.
+  unsigned int frustum_phase_ = 0;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
   {

plugins/voxel_layer.cpp

@@ -98,6 +98,8 @@ 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);
+    obstacle_decay_time_ = loadParam(layer, "obstacle_decay_time", 0.0);
+    frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
 
     int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
     if (nh.hasParam("enabled"))
@@ -125,6 +127,18 @@ 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("obstacle_decay_time"))
+      nh.getParam("obstacle_decay_time", obstacle_decay_time_);
+    if (nh.hasParam("frustum_skip_distance"))
+      nh.getParam("frustum_skip_distance", frustum_skip_distance_);
+
+    if (obstacle_decay_time_ > 0.0)
+    {
+      robot::log_info(
+          "VoxelLayer obstacle decay enabled: LETHAL cells not re-observed for %.1f s are freed. "
+          "Intended for camera-only costmaps; decay clears obstacles outside the current FOV.\n",
+          obstacle_decay_time_);
+    }
 
     this->matchSize();
   } 
@@ -140,6 +154,7 @@ void VoxelLayer::matchSize()
 {
   ObstacleLayer::matchSize();
   robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
+  cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
   if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_)) 
   {
     std::cerr << "[FATAL] Voxel grid size mismatch: "
@@ -161,6 +176,7 @@ void VoxelLayer::resetMaps()
 {
   Costmap2D::resetMaps();
   robot_voxel_grid_.reset();
+  cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
 }
 
 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -197,6 +213,8 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
   }
 
   // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  const double now_sec =
+      obstacle_decay_time_ > 0.0 ? robot::Time::now().toSec() : 0.0;
   for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
   {
     const Observation& obs = *it;
@@ -242,13 +260,56 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
         unsigned int index = getIndex(mx, my);
 
         costmap_[index] = LETHAL_OBSTACLE;
+        if (obstacle_decay_time_ > 0.0)
+          cell_last_marked_[index] = now_sec;
         touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
       }
     }
   }
+
+  if (obstacle_decay_time_ > 0.0)
+    decayStaleObstacles(now_sec, min_x, min_y, max_x, max_y);
+
   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
 }
 
+void VoxelLayer::decayStaleObstacles(double now_sec, double* min_x, double* min_y,
+                                     double* max_x, double* max_y)
+{
+  const std::size_t map_size = static_cast<std::size_t>(size_x_) * size_y_;
+  if (cell_last_marked_.size() != map_size)
+    cell_last_marked_.assign(map_size, 0.0);
+
+  for (std::size_t index = 0; index < map_size; ++index)
+  {
+    if (costmap_[index] != LETHAL_OBSTACLE)
+      continue;
+
+    double& stamp = cell_last_marked_[index];
+    // A LETHAL cell without a stamp was marked before decay tracking covered
+    // it (e.g. right after a resize). Start its timer now instead of freeing
+    // an obstacle we have no age evidence for.
+    if (stamp <= 0.0)
+    {
+      stamp = now_sec;
+      continue;
+    }
+
+    if (now_sec - stamp <= obstacle_decay_time_)
+      continue;
+
+    costmap_[index] = FREE_SPACE;
+    robot_voxel_grid_.clearVoxelColumn(static_cast<unsigned int>(index));
+    stamp = 0.0;
+
+    unsigned int mx, my;
+    indexToCells(static_cast<unsigned int>(index), mx, my);
+    double wx, wy;
+    mapToWorld(mx, my, wx, wy);
+    touch(wx, wy, min_x, min_y, max_x, max_y);
+  }
+}
+
 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
 {
   // get the cell coordinates of the center point of the window
@@ -458,11 +519,13 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned
 
 void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
                                      unsigned int pixel_step, double fx, double fy,
-                                     double cx, double cy)
+                                     double cx, double cy,
+                                     unsigned int u_offset, unsigned int v_offset)
 {
   if (cached_depth_width_ == width && cached_depth_height_ == height &&
       cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
-      cached_cx_ == cx && cached_cy_ == cy)
+      cached_cx_ == cx && cached_cy_ == cy &&
+      cached_u_offset_ == u_offset && cached_v_offset_ == v_offset)
   {
     return;
   }
@@ -474,15 +537,17 @@ void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
   cached_fy_ = fy;
   cached_cx_ = cx;
   cached_cy_ = cy;
+  cached_u_offset_ = u_offset;
+  cached_v_offset_ = v_offset;
 
   const std::size_t rows = (height + pixel_step - 1) / pixel_step;
   const std::size_t columns = (width + pixel_step - 1) / pixel_step;
   depth_ray_cache_.clear();
   depth_ray_cache_.reserve(rows * columns);
 
-  for (unsigned int v = 0; v < height; v += pixel_step)
+  for (unsigned int v = v_offset; v < height; v += pixel_step)
   {
-    for (unsigned int u = 0; u < width; u += pixel_step)
+    for (unsigned int u = u_offset; u < width; u += pixel_step)
     {
       const double x = (static_cast<double>(u) - cx) / fx;
       const double y = (static_cast<double>(v) - cy) / fy;
@@ -618,10 +683,20 @@ 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);
+
+  // Dither the sampled pixel grid every pass. With a fixed subgrid a static
+  // camera re-traces identical rays each frame; cells between adjacent rays
+  // (angular gap step / fx, ~5 cm at 2.5 m for step 8) are never crossed and
+  // stay marked until the robot moves. Cycling the phase sweeps every pixel
+  // column/row over step^2 frames at the same per-frame ray count.
+  const unsigned int u_offset = step > 1 ? frustum_phase_ % step : 0;
+  const unsigned int v_offset = step > 1 ? (frustum_phase_ / step) % step : 0;
+  ++frustum_phase_;
+  updateDepthRayCache(width, height, step, fx, fy, cx, cy, u_offset, v_offset);
 
   double qx = tfm.transform.rotation.x;
   double qy = tfm.transform.rotation.y;
@@ -715,8 +790,10 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
   const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
   rolling_costmap_scratch_.resize(overlap_size);
   rolling_voxel_scratch_.resize(overlap_size);
+  rolling_stamp_scratch_.resize(overlap_size);
   unsigned char* local_map = rolling_costmap_scratch_.data();
   unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
+  double* local_stamp_map = rolling_stamp_scratch_.data();
   unsigned int* voxel_map = robot_voxel_grid_.getData();
 
   if (overlap_size > 0)
@@ -725,6 +802,8 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
                   cell_size_x, cell_size_x, cell_size_y);
     copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
                   cell_size_x, cell_size_x, cell_size_y);
+    copyMapRegion(cell_last_marked_.data(), lower_left_x, lower_left_y, size_x_,
+                  local_stamp_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
   }
 
   // we'll reset our maps to unknown space if appropriate
@@ -745,6 +824,8 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
                   size_x_, cell_size_x, cell_size_y);
     copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
                   size_x_, cell_size_x, cell_size_y);
+    copyMapRegion(local_stamp_map, 0, 0, cell_size_x, cell_last_marked_.data(),
+                  start_x, start_y, size_x_, cell_size_x, cell_size_y);
   }
 
 }