How can I extract free spaces from a occupancy grid map in the MATLAB. I have the following code which gave the output that there is no free cell on the occupancy grid map.
clc;
clear;
close all;
rng(1);
% read the PCD file as pointcloud
ptCloud = pcread("my.pcd");
% rotate pointcloud (optional)
rotationAngles = [0.0, 0.0, 0.0];
translation = [0.0, 0.0, 0.0];
tform = rigidtform3d(rotationAngles,translation);
ptCloud = pctransform(ptCloud,tform);
% create occupancy grid using point cloud
map3D = occupancyMap3D(3);
map3D.FreeThreshold = 0.5;
pose = [0.0 0.0 0.0 1.0 0.0 0.0 0.0];
maxRange = 15;
insertPointCloud(map3D,pose,ptCloud,maxRange);
% Show the occupancy map (initial visualization)
figure;
show(map3D);
axis("equal");
title('Occupancy Map Visualization');
hold on;
x_limits = ptCloud.XLimits;
y_limits = ptCloud.YLimits;
z_limits = ptCloud.ZLimits;
% Define the resolution you are using for the occupancy map
resolution = 3;
% Create a grid of points to sample within the bounding box
[X, Y, Z] = meshgrid(x_limits(1):resolution:x_limits(2), ...
y_limits(1):resolution:y_limits(2), ...
z_limits(1):resolution:z_limits(2));
% Reshape the grid coordinates into an N-by-3 matrix
xyz_sample_points = [X(:), Y(:), Z(:)];
% Get the occupancy values for ALL the sample points
% This calls the getOccupancy function many times internally
occVal = getOccupancy(map3D, xyz_sample_points);
% Select only the points that are marked as "free" (value 0)
freeSpaceIdx = (occVal == 0);
freeSpacePoints = xyz_sample_points(freeSpaceIdx, :);
if isempty(freeSpacePoints)
disp('No free space points found within the original point cloud bounds.');
else
% Create a pointCloud object from the free space points
ptCloudFree = pointCloud(freeSpacePoints);
% Visualize the extracted free space point cloud
figure;
pcshow(ptCloudFree);
title('Extracted Free Space Point Cloud');
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
grid on;
end
Output : No free space points found within the original point cloud bounds.
I believe the output is erroneous. Can anyone help me to point out the error? Thank you.