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

:optimal test NUC 27/7 15:14

Browse Source
This commit is contained in:
duongtd
2026-07-27 15:14:39 +07:00
parent c17ac9fc06
commit 9f7e2f82f1
5 changed files with 39 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
plugins/voxel_layer.cpp
View File

@@ -744,15 +744,21 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
continue;
// Left-edge stereo no-disparity strip. An INVALID pixel here is structurally
// invalid (carries no free-space evidence), so clearing it out to max_range
// erases obstacles rotating out of the FOV on that side (the CW-turn bug) —
// skip only those. A VALID return here is a real measured surface, so it must
// still clear normally; otherwise the whole left border becomes a clearing
// dead zone and obstacles there never get cleared. Invalid pixels OUTSIDE the
// strip still clear to max_range (ghost removal).
// Edge stereo no-disparity strips (left and/or right, depending on the
// camera). An INVALID pixel in such a strip is structurally invalid (carries
// no free-space evidence), so clearing it out to max_range erases obstacles
// rotating out of the FOV on that side (the turn bug) — skip only those. A
// VALID return there is a real measured surface, so it must still clear
// normally; otherwise the border becomes a clearing dead zone and obstacles
// there never get cleared. Invalid pixels OUTSIDE the strips still clear to
// max_range (ghost removal). Right edge measured inward from the last column;
// the unsigned test avoids underflow when the border exceeds the width.
const bool in_left_border = local_ray.u < frustum.clear_left_border_px;
if (in_left_border && !valid)
const bool in_right_border =
frustum.clear_right_border_px > 0 &&
local_ray.u + frustum.clear_right_border_px >= width;
const bool in_border = in_left_border || in_right_border;
if (in_border && !valid)
continue;
// depth images store z-depth; local_ray.z is the unit ray's optical axis
@@ -779,8 +785,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
stat.best_row_delta = row_delta;
}
stat.has_ray = true;
if (in_left_border)
stat.in_left_border = true;
if (in_border)
stat.in_border = true;
if (valid)
{
@@ -904,11 +910,11 @@ bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
if (!stat.has_ray)
continue;
// In the left stereo strip, never clear a whole column out to max_range on a
// In an edge stereo strip, never clear a whole column out to max_range on a
// missing in-band return: that is exactly the no-free-space-evidence case that
// erases obstacles turning out of view. Only an in-band measured surface may
// shorten (and thus clear) a border column.
if (stat.in_left_border && stat.min_band_dist < 0.0)
if (stat.in_border && stat.min_band_dist < 0.0)
continue;
double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;