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

optimal voxel layer

Browse Source
This commit is contained in:
duongtd
2026-07-23 11:39:03 +07:00
parent 28a32ee67c
commit f0390fcc67
2 changed files with 28 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
include/robot_costmap_2d/voxel_layer.h
View File

@@ -97,7 +97,8 @@ private:
bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
double& depth_m, bool& is_valid) const;
void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
double fx, double fy, double cx, double cy);
double fx, double fy, double cx, double cy,
unsigned int u_offset, unsigned int v_offset);
bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
double raytrace_range, unsigned int cell_raytrace_range,
@@ -144,6 +145,14 @@ private:
double cached_fy_ = 0.0;
double cached_cx_ = 0.0;
double cached_cy_ = 0.0;
unsigned int cached_u_offset_ = 0;
unsigned int cached_v_offset_ = 0;
/// Advances every frustum pass to dither the sampled pixel grid. A static
/// camera otherwise re-traces the same fixed pixel_step subgrid each frame,
/// so cells between adjacent rays (angular gap pixel_step / fx) are never
/// crossed and stay marked until the robot moves. Cycling the grid phase
/// sweeps every pixel over pixel_step^2 frames at unchanged per-frame cost.
unsigned int frustum_phase_ = 0;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
{