%% Check If Sequence of Poses Enters Occupied Cell
% Load a costmap from a parking lot.

data = load('parkingLotCostmap.mat');
parkMap = data.parkingLotCostmap;
plot(parkMap)

% Create vehicle poses following a straight-line path. x and y are the (x,y) coordinates of the rear axle of the vehicle. theta is the angle of the rear axle with respect to the x-axis. Note that the dimensions of the vehicle are stored in the vehicleDimensions property of the costmap, and that there is an offset between the rear axle of the vehicle and its center.

x = 6:0.25:10;
y = repmat(5,size(x));
theta = zeros(size(x));
vehiclePoses = [x',y',theta'];
hold on
plot(x,y,'b.')

% Check if the poses are occupied.

occ = checkOccupied(parkMap,vehiclePoses)

% The vehicle poses are occupied beginning with the sixth pose. In other words, the center of the vehicle in the sixth pose lies within the inflation radius of an occupied grid cell.
% Copyright 2012 The MathWorks, Inc.