1

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.

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.