From 0e84ac53cbf52bcd7843f4eab05addb9c67b5c9f Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 23 Jul 2026 14:47:27 +0700 Subject: [PATCH] otimal --- include/robot_costmap_2d/voxel_layer.h | 34 ++++ plugins/voxel_layer.cpp | 245 ++++++++++++++++++++++++- 2 files changed, 273 insertions(+), 6 deletions(-) diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index 1601dc4..d4148c0 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -51,6 +51,9 @@ #include #include +#include +#include + namespace robot_costmap_2d { @@ -102,11 +105,42 @@ private: bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz, double raytrace_range, unsigned int cell_raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y); + bool clearDepthColumns(double ox, double oy, double cover_distance, double far_distance, + double min_range, double max_range, double skip_dist, + double* min_x, double* min_y, double* max_x, double* max_y); + bool clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const; bool publish_voxel_; robot_voxel_grid::VoxelGrid robot_voxel_grid_; double z_resolution_, origin_z_; + /// Clearing rays stop this far [m] before the measured surface. + /// Negative keeps the legacy 2 * resolution behaviour. + double frustum_skip_distance_ = -1.0; + + /// Full-column clearing: per depth-image pixel column, the nearest return + /// inside the obstacle height band certifies "no obstacle in this direction + /// closer than d". Cells along that 2D beam get their whole voxel column + /// cleared, removing marked voxels the per-pixel 3D rays cannot reach + /// (above the vertical FOV at close range). + bool frustum_column_clearing_ = false; + /// Height band [m] used to detect the nearest in-band return. Points below + /// the band (floor) do not shorten the beam. max < 0: use max_obstacle_height. + double column_clear_min_height_ = 0.10; + double column_clear_max_height_ = -1.0; + /// Full columns are only cleared beyond this distance [m], where the + /// vertical FOV covers the whole height band. Negative: derive each frame + /// from the camera intrinsics and mounting pose. + double column_cover_distance_ = -1.0; + + struct DepthColumnStat + { + 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 + }; + std::vector depth_column_stats_; unsigned int unknown_threshold_, mark_threshold_, size_z_; robot_sensor_msgs::PointCloud clearing_endpoints_; std::vector rolling_costmap_scratch_; diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index f21d910..e379df5 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -98,6 +98,11 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_); mark_threshold_ = loadParam(layer, "mark_threshold", 0); combination_method_ = loadParam(layer, "combination_method", 0.0); + frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0); + frustum_column_clearing_ = loadParam(layer, "frustum_column_clearing", false); + column_clear_min_height_ = loadParam(layer, "column_clear_min_height", 0.10); + column_clear_max_height_ = loadParam(layer, "column_clear_max_height", -1.0); + column_cover_distance_ = loadParam(layer, "column_cover_distance", -1.0); int size_z, unknown_threshold, mark_threshold, frustum_pixel_step; if (nh.hasParam("enabled")) @@ -125,6 +130,26 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl } if (nh.hasParam("combination_method")) nh.getParam("combination_method", combination_method_); + if (nh.hasParam("frustum_skip_distance")) + nh.getParam("frustum_skip_distance", frustum_skip_distance_); + if (nh.hasParam("frustum_column_clearing")) + nh.getParam("frustum_column_clearing", frustum_column_clearing_); + if (nh.hasParam("column_clear_min_height")) + nh.getParam("column_clear_min_height", column_clear_min_height_); + if (nh.hasParam("column_clear_max_height")) + nh.getParam("column_clear_max_height", column_clear_max_height_); + if (nh.hasParam("column_cover_distance")) + nh.getParam("column_cover_distance", column_cover_distance_); + + robot::log_info("VoxelLayer frustum_skip_distance: %.3f m%s\n", + frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_, + frustum_skip_distance_ >= 0.0 ? "" : " (legacy 2 * resolution)"); + robot::log_info("VoxelLayer frustum_column_clearing: %s, band: [%.2f, %.2f] m, cover_distance: %.2f m%s\n", + frustum_column_clearing_ ? "true" : "false", + column_clear_min_height_, + column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_, + column_cover_distance_, + column_cover_distance_ >= 0.0 ? "" : " (auto from camera pose)"); this->matchSize(); } @@ -618,7 +643,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, const unsigned int step = std::max(1u, observation.pixel_step_); const double min_range = observation.min_range_; const double max_range = observation.max_range_; - const double skip_dist = 2.0 * resolution_; + const double skip_dist = + frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_; const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width); const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height); updateDepthRayCache(width, height, step, fx, fy, cx, cy); @@ -647,6 +673,65 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, const unsigned int cell_raytrace_range = cellDistance(max_range); bool cleared_any = false; + // Column clearing: certify the beam length per pixel column and the distance + // window [cover, far] where the vertical FOV spans the whole height band. + // Outside that window a real obstacle could sit above/below the FOV, so only + // the per-pixel 3D rays may clear there. + const double band_min_h = column_clear_min_height_; + const double band_max_h = + column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_; + double cover_dist = column_cover_distance_; + double far_dist = std::numeric_limits::infinity(); + bool column_pass = frustum_column_clearing_ && band_max_h > band_min_h; + + if (column_pass && cover_dist < 0.0) + { + const double up_half = std::atan2(cy, fy); + const double down_half = std::atan2(static_cast(height) - 1.0 - cy, fy); + const double axis_elev = std::atan2(r22, std::hypot(r02, r12)); + const double alpha_top = axis_elev + up_half; + const double alpha_bot = axis_elev - down_half; + const double band_top = band_max_h - oz; + const double band_bot = band_min_h - oz; + constexpr double kMinSlope = 1e-3; + + cover_dist = 0.0; + if (band_top > 0.0) + { + if (alpha_top <= kMinSlope) + column_pass = false; // camera can never look up to the band top + else + cover_dist = std::max(cover_dist, band_top / std::tan(alpha_top)); + } + else if (alpha_top < -kMinSlope) + { + far_dist = std::min(far_dist, band_top / std::tan(alpha_top)); + } + if (band_bot < 0.0) + { + if (alpha_bot >= -kMinSlope) + column_pass = false; // camera can never look down to the band bottom + else + cover_dist = std::max(cover_dist, band_bot / std::tan(alpha_bot)); + } + else if (alpha_bot > kMinSlope) + { + far_dist = std::min(far_dist, band_bot / std::tan(alpha_bot)); + } + + if (!column_pass) + { + robot::log_warning_throttle( + 10.0, "VoxelLayer column clearing disabled: vertical FOV [%.1f, %.1f] deg at camera " + "height %.2f m never covers band [%.2f, %.2f] m\n", + alpha_bot * 180.0 / M_PI, alpha_top * 180.0 / M_PI, oz, band_min_h, band_max_h); + } + } + + const unsigned int column_count = (width + step - 1) / step; + if (column_pass) + depth_column_stats_.assign(column_count, DepthColumnStat()); + for (const DepthRay& local_ray : depth_ray_cache_) { double depth_m = 0.0; @@ -654,6 +739,47 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid)) continue; + robot_geometry_msgs::Vector3 global_ray; + global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z; + global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z; + global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z; + + if (column_pass) + { + const double horiz_norm = std::hypot(global_ray.x, global_ray.y); + const unsigned int col = local_ray.u / step; + if (horiz_norm > 1e-6 && col < column_count) + { + DepthColumnStat& stat = depth_column_stats_[col]; + const double row_delta = std::fabs(static_cast(local_ray.v) - cy); + if (stat.min_band_dist < 0.0 && row_delta < stat.best_row_delta) + { + // no in-band return yet: aim the beam along the ray nearest the + // principal row + stat.azimuth = std::atan2(global_ray.y, global_ray.x); + stat.best_row_delta = row_delta; + } + stat.has_ray = true; + + if (valid) + { + // depth images store z-depth; local_ray.z is the unit ray's optical + // axis component, so depth / z is the Euclidean range + const double euclid_range = depth_m / local_ray.z; + const double pz = oz + global_ray.z * euclid_range; + if (pz >= band_min_h && pz <= band_max_h) + { + const double dist_h = horiz_norm * euclid_range; + if (stat.min_band_dist < 0.0 || dist_h < stat.min_band_dist) + { + stat.min_band_dist = dist_h; + stat.azimuth = std::atan2(global_ray.y, global_ray.x); + } + } + } + } + } + double ray_len = max_range; if (valid && depth_m < max_range) ray_len = std::max(0.0, depth_m - skip_dist); @@ -661,11 +787,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, if (ray_len <= min_range) continue; - robot_geometry_msgs::Vector3 global_ray; - global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z; - global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z; - global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z; - const double sx = ox + global_ray.x * min_range; const double sy = oy + global_ray.y * min_range; const double sz = oz + global_ray.z * min_range; @@ -677,6 +798,118 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, min_x, min_y, max_x, max_y) || cleared_any; } + if (column_pass) + { + cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range, + skip_dist, min_x, min_y, max_x, max_y) || + cleared_any; + } + + return cleared_any; +} + +namespace +{ +/// raytraceLine action: frees the 2D cell and wipes its whole voxel column. +class ClearFullColumn +{ +public: + ClearFullColumn(unsigned char* costmap, robot_voxel_grid::VoxelGrid& voxel_grid) + : costmap_(costmap), voxel_grid_(voxel_grid) + { + } + + inline void operator()(unsigned int offset) + { + costmap_[offset] = FREE_SPACE; + voxel_grid_.clearVoxelColumn(offset); + } + +private: + unsigned char* costmap_; + robot_voxel_grid::VoxelGrid& voxel_grid_; +}; +} // namespace + +bool VoxelLayer::clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const +{ + // Liang-Barsky clip against the map interior; the half-resolution margin + // keeps clipped endpoints valid for worldToMap. + const double min_wx = origin_x_; + const double min_wy = origin_y_; + const double max_wx = origin_x_ + getSizeInMetersX() - 0.5 * resolution_; + const double max_wy = origin_y_ + getSizeInMetersY() - 0.5 * resolution_; + const double dx = ex - sx; + const double dy = ey - sy; + const double p[4] = {-dx, dx, -dy, dy}; + const double q[4] = {sx - min_wx, max_wx - sx, sy - min_wy, max_wy - sy}; + + double t0 = 0.0; + double t1 = 1.0; + for (int i = 0; i < 4; ++i) + { + if (std::fabs(p[i]) < 1e-12) + { + if (q[i] < 0.0) + return false; + continue; + } + const double r = q[i] / p[i]; + if (p[i] < 0.0) + t0 = std::max(t0, r); + else + t1 = std::min(t1, r); + } + if (t0 > t1) + return false; + + const double bx = sx; + const double by = sy; + sx = bx + t0 * dx; + sy = by + t0 * dy; + ex = bx + t1 * dx; + ey = by + t1 * dy; + return true; +} + +bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance, + double far_distance, double min_range, double max_range, + double skip_dist, double* min_x, double* min_y, + double* max_x, double* max_y) +{ + const double start_dist = std::max(cover_distance, min_range); + bool cleared_any = false; + + for (const DepthColumnStat& stat : depth_column_stats_) + { + if (!stat.has_ray) + 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) + continue; + + const double cos_az = std::cos(stat.azimuth); + const double sin_az = std::sin(stat.azimuth); + double sx = ox + cos_az * start_dist; + double sy = oy + sin_az * start_dist; + double ex = ox + cos_az * end_dist; + double ey = oy + sin_az * end_dist; + if (!clipColumnSegment(sx, sy, ex, ey)) + continue; + + unsigned int sx_m, sy_m, ex_m, ey_m; + if (!worldToMap(sx, sy, sx_m, sy_m) || !worldToMap(ex, ey, ex_m, ey_m)) + continue; + + ClearFullColumn clearer(costmap_, robot_voxel_grid_); + raytraceLine(clearer, sx_m, sy_m, ex_m, ey_m); + touch(sx, sy, min_x, min_y, max_x, max_y); + touch(ex, ey, min_x, min_y, max_x, max_y); + cleared_any = true; + } + return cleared_any; }