How do I shift the plot of a matrix in a Binary Occupancy Map?

I am building a binary occupancy map for robot path planning. The outer border is the boundary and a square with four given points. There is one obstacle inside the boundary, a square, with four given points. I do not know how to assign occupancy values for points/coordinates that do not begin at the origin. All occupancy values are 1. Right now I can only plot and assign occupancy values for the far left and bottom lines of the boundary's square shape. I do not know how to get the top and the right as they do not start at the origin. How do I assign values starting from the coordinate (0,120) across to (120,120)? This is what I have:

%Occupancy grid size 120x120 with resolution 1

map = binaryOccupancyMap(120,120,1,"grid")

%First four coordinates are boundaries of environment. Last four are boundaries of the obstacle.

%x = [0; 0; 120; 120; 20; 100; 20; 100];

%y = [0; 120; 120; 0; 100; 100; 20; 20];

x = 1:1:120;

y = ones(1,120);

x1 = ones(1,120);

y1 = 1:1:120;

x2 = 1:1:120;

y2 = ones(1,120);

topLeft = [120,0];

setOccupancy(map, [x' y'], ones(120,1))

setOccupancy(map, [x1' y1'], ones (1,120))

setOccupancy(map,topLeft,y2,"local") %This is my attempt to get the top line but it does not work

figure

show(map)

Answer 1

[Cameron Stabile]

Hi Analise,

When using the block syntax for setOccupancy, the first input should be the minimum corner of your block (i.e. bottom-left corner for XY coordinates, top-left corner if you are using grid coordinates), and the second input is the block of values themselves, which determine the size of the block.

In your example, you have a few options:

1) The easiest option would be to add an occupied block, then remove the center

% Occupancy grid size 120x120 with resolution 1
map1 = binaryOccupancyMap(120,120,1,"grid");

%First four coordinates are boundaries of environment. Last four are boundaries of the obstacle. 
%x = [0; 0; 120; 120; 20; 100; 20; 100];
%y = [0; 120; 120; 0; 100; 100; 20; 20];

% Define outside of block in local-coordinates
cornerLocal = [20 20];
blockSize = map1.GridSize/map1.Resolution-cornerLocal;
blockValues = true(blockSize);
setOccupancy(map1,cornerLocal,blockValues,"local");
figure(1); subplot(1,2,1); title('setOccupancy Block: Area Filled');
show(map1)

% Un-occupy the center
setOccupancy(map1,cornerLocal+1,~blockValues(2:end-1,2:end-1),"local");
subplot(1,2,2); title('setOccupancy Block: Inside removed');
show(map1)

2) Another option is to add the individual lines, which follows the same rules as before

%Occupancy grid size 120x120 with resolution 1
map2 = binaryOccupancyMap(120,120,1,"grid");

% Define 4 corners and edge-lengths, in [left, top, right, bottom] order
edgeSize = ones(4,2);
m = [1;0;1;0] == true;
edgeSize([m ~m]) = repmat(blockSize,1,2);
blockCorners = [cornerLocal;
                cornerLocal+[0 blockSize(1)-1]; % Note that we need to make the corners
                cornerLocal+[blockSize(2)-1 0]; % bottom-left for each edge
                cornerLocal];
figure(2); ax = gca; 
for i = 1:size(blockCorners,1)
    setOccupancy(map2,blockCorners(i,:),ones(edgeSize(i,:)),'local');
    show(map2,'Parent',ax);
    title('setOccupancy Block: Edge-by-edge')
end

3) Lastly, if your "walls" will only be 1-cell wide, then an alternative to #2 is to use raycast. This has the added benefit of working on rotated rectangles or arbitrary polygonal shapes.

%Occupancy grid size 120x120 with resolution 1
map3 = binaryOccupancyMap(120,120,1,"grid");

% Define center of bottom-left corner's cell in world coordinates
cellSize = 1/map3.Resolution;
edgeLength = fliplr(edgeSize)/cellSize;
startPts = local2world(map3,blockCorners+cellSize/2);
endPts   = startPts + edgeLength-cellSize;

% Insert edges
figure(3); ax = gca;
for i = 1:size(startPts,1)
    [endRayPts,midRayPts] = raycast(map3,startPts(i,:),endPts(i,:));
    % Note that output is in grid indices, so we use the point version of 
    % setOccupancy with "grid" as  frame
    setOccupancy(map3,[endRayPts;midRayPts],true,'grid'); 
    show(map3,'Parent',ax);
    title('raycast+setOccupancy: Raycast edges');
end

Hope this helps,

Cameron