diff --git a/include/robot_costmap_2d/observation.h b/include/robot_costmap_2d/observation.h index 78669d6..75fbca7 100755 --- a/include/robot_costmap_2d/observation.h +++ b/include/robot_costmap_2d/observation.h @@ -70,6 +70,13 @@ struct DepthFrustumConfig /// Full columns are only cleared beyond this distance [m]. Negative: /// derive each frame from the camera intrinsics and mounting pose. double column_cover_distance = -1.0; + /// Depth-image columns at the LEFT edge excluded from clearing [px]. Covers + /// the stereo no-disparity strip that is permanently invalid there: those + /// pixels carry no free-space evidence, so clearing through them erases + /// obstacles that rotate out of the FOV on that side. Set to the measured + /// width of the black strip in the raw depth image (a few px margin). 0 + /// disables. Invalid pixels ELSEWHERE still clear (needed for ghost removal). + unsigned int clear_left_border_px = 0; }; class DepthCameraObservation diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index d235353..bcfd113 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -99,12 +99,6 @@ 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); @@ -131,8 +125,7 @@ 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_valid_return = false; ///< column had at least one valid depth measurement + bool has_ray = false; ///< column had at least one readable pixel }; std::vector depth_column_stats_; unsigned int unknown_threshold_, mark_threshold_, size_z_; diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 5614288..7c4851a 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -168,6 +168,9 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance); frustum_config.column_cover_distance = loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance); + int frustum_clear_left_border = + loadParam(layer[source], "frustum_clear_left_border_px", + loadParam(layer, "frustum_clear_left_border_px", 0)); if (priv_nh.hasParam("topic")) priv_nh.getParam("topic", topic); @@ -212,10 +215,14 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance); if (priv_nh.hasParam("column_cover_distance")) priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance); + if (priv_nh.hasParam("frustum_clear_left_border_px")) + priv_nh.getParam("frustum_clear_left_border_px", frustum_clear_left_border); if (priv_nh.hasParam("frustum_depth_camera_topic")) priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_); frustum_config.pixel_step = static_cast(std::max(1, frustum_pixel_step)); + frustum_config.clear_left_border_px = + static_cast(std::max(0, frustum_clear_left_border)); robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, " "skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, " diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index b758bb1..78aa738 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -57,19 +57,6 @@ 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() { @@ -469,36 +456,6 @@ 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) @@ -782,6 +739,14 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, for (const DepthRay& local_ray : depth_ray_cache_) { + // Skip the left-edge stereo no-disparity strip: those columns are + // permanently invalid, so clearing through them erases obstacles rotating + // out of the FOV on that side. Marking has no data there either, so nothing + // is lost. Invalid pixels ELSEWHERE still clear to max_range (ghost removal + // when an obstacle leaves and only far / open space remains behind it). + if (local_ray.u < frustum.clear_left_border_px) + continue; + double depth_m = 0.0; bool valid = false; if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid)) @@ -814,7 +779,6 @@ 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) { @@ -829,14 +793,6 @@ 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); @@ -943,13 +899,6 @@ 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)