MATLAB中3D占用栅格地图自由空间提取失败问题排查

问题排查与修复方案 #

1. 错误的自由空间判断逻辑 #

代码中用occVal == 0筛选自由空间，但MATLAB的occupancyMap3D中，自由体素的占据概率是低于设定的FreeThreshold（而非严格等于0）。未观测的体素默认概率为0.5，已标记为自由的体素概率会低于阈值，不会是绝对的0。

修复：将判断条件改为基于阈值的范围筛选：

freeSpaceIdx = (occVal < map3D.FreeThreshold); 

2. 采样范围未覆盖实际自由空间 #

当前采样仅局限于原始点云的边界内，但insertPointCloud是从传感器位姿（此处为原点[0,0,0]）向点云发射射线，标记射线沿途的体素为自由空间。这些自由空间通常位于原点到点云点之间，不在点云自身的边界范围内。

修复：扩展采样范围，包含原点到点云边界的区域：

% 扩展X/Y/Z范围，覆盖原点到点云的区域
x_limits = [min(0, ptCloud.XLimits(1)), ptCloud.XLimits(2)];
y_limits = [min(0, ptCloud.YLimits(1)), ptCloud.YLimits(2)];
z_limits = [min(0, ptCloud.ZLimits(1)), ptCloud.ZLimits(2)];

3. 采样点与体素中心未对齐 #

创建采样网格时，直接使用体素分辨率作为步长，导致采样点落在体素边缘而非中心。边缘点可能落入未观测的体素（概率0.5），无法被识别为自由空间。

修复：采样体素中心位置：

% 计算体素中心的采样坐标
x_steps = x_limits(1) + resolution/2 : resolution : x_limits(2) - resolution/2;
y_steps = y_limits(1) + resolution/2 : resolution : y_limits(2) - resolution/2;
z_steps = z_limits(1) + resolution/2 : resolution : z_limits(2) - resolution/2;
[X, Y, Z] = meshgrid(x_steps, y_steps, z_steps);

4. 占据阈值与插入参数不合理 #

• FreeThreshold=0.5过高：默认自由阈值为0.2，0.5会导致只有概率极低的体素才被判定为自由，大部分自由体素会被排除。建议调回默认值：

  map3D.FreeThreshold = 0.2;
  

• maxRange=15限制过严：若点云存在超出15米的点，这些点会被忽略，对应方向的自由空间无法标记。需覆盖点云全部范围：

  maxRange = max(pcDistance(ptCloud, [0,0,0])); % 设为点云到原点的最大距离
  

核心修复后的代码片段 #

% 创建3D占用栅格地图
map3D = occupancyMap3D(3);
map3D.FreeThreshold = 0.2; % 恢复默认自由阈值
pose = [0.0 0.0 0.0 1.0 0.0 0.0 0.0];
maxRange = max(pcDistance(ptCloud, [0,0,0])); % 覆盖所有点云范围
insertPointCloud(map3D,pose,ptCloud,maxRange);

% 扩展采样范围至原点到点云区域
x_limits = [min(0, ptCloud.XLimits(1)), ptCloud.XLimits(2)];
y_limits = [min(0, ptCloud.YLimits(1)), ptCloud.YLimits(2)];
z_limits = [min(0, ptCloud.ZLimits(1)), ptCloud.ZLimits(2)];

% 采样体素中心
resolution = 3; 
x_steps = x_limits(1) + resolution/2 : resolution : x_limits(2) - resolution/2;
y_steps = y_limits(1) + resolution/2 : resolution : y_limits(2) - resolution/2;
z_steps = z_limits(1) + resolution/2 : resolution : z_limits(2) - resolution/2;
[X, Y, Z] = meshgrid(x_steps, y_steps, z_steps);

xyz_sample_points = [X(:), Y(:), Z(:)];
occVal = getOccupancy(map3D, xyz_sample_points);

% 筛选自由空间点
freeSpaceIdx = (occVal < map3D.FreeThreshold); 
freeSpacePoints = xyz_sample_points(freeSpaceIdx, :);

内容的提问来源于stack exchange，提问作者Encipher