diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index bcfd113..d235353 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -99,6 +99,12 @@ 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); @@ -125,7 +131,8 @@ private: 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::infinity(); - bool has_ray = false; ///< column had at least one readable pixel + 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 depth_column_stats_; unsigned int unknown_threshold_, mark_threshold_, size_z_; diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index e8df3db..b758bb1 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -57,6 +57,19 @@ 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() { @@ -456,6 +469,36 @@ 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(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(u) + off[0]; + const long nv = static_cast(v) + off[1]; + if (nu < 0 || nv < 0) + continue; + double neighbor_depth = 0.0; + bool neighbor_valid = false; + if (readDepthMeters(depth, static_cast(nu), static_cast(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) @@ -771,6 +814,7 @@ 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) { @@ -785,6 +829,14 @@ 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); @@ -891,6 +943,13 @@ 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)