diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index 8fcb9b9..61ec5f2 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/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) { diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index d93924f..2dc021b 100755 --- a/plugins/voxel_layer.cpp +++ b/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(u) - cx) / fx; const double y = (static_cast(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;