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

:optimal

Browse Source
This commit is contained in:
duongtd
2026-07-27 11:22:49 +07:00
parent e2ee28bd63
commit e3b52765c1
2 changed files with 67 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
plugins/voxel_layer.cpp
View File

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