optimal voxel layer

include/robot_costmap_2d/voxel_layer.h

@@ -97,7 +97,8 @@ 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,
@@ -144,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

@@ -519,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;
   }
@@ -535,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;
@@ -683,7 +687,16 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
       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;