DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

:optimal 27/7 12:11

Browse Source
This commit is contained in:
duongtd
2026-07-27 12:11:14 +07:00
parent e3b52765c1
commit 5da5421ec7
4 changed files with 23 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
include/robot_costmap_2d/observation.h
View File

@@ -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

7
include/robot_costmap_2d/voxel_layer.h
View File

@@ -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);
@@ -132,7 +126,6 @@ private:
double azimuth = 0.0; ///< beam direction in the global frame
double best_row_delta = std::numeric_limits<double>::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
};
std::vector<DepthColumnStat> depth_column_stats_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;

7
plugins/obstacle_layer.cpp
View File

@@ -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<unsigned int>(std::max(1, frustum_pixel_step));
frustum_config.clear_left_border_px =
static_cast<unsigned int>(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, "

67
plugins/voxel_layer.cpp
View File

@@ -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<int>(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<long>(u) + off[0];
const long nv = static_cast<long>(v) + off[1];
if (nu < 0 || nv < 0)
continue;
double neighbor_depth = 0.0;
bool neighbor_valid = false;
if (readDepthMeters(depth, static_cast<unsigned int>(nu), static_cast<unsigned int>(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)