Preoccupy BinaryOccupancyMap or invert occupancy

I am trying to create a binaryOccupancyMap from scratch and to do so, I would like to first occupy the whole map. This does not really seem to be easily possible. I tried set the DefaultValue to 1 instezd of 0, but this has no effect on the map itself, only on points outside the map. If I try to set a point in the center of the map to occupied and then inflate that point to cover the whole map, the map needs ages to load, which cannot be the answer either. So my question is:

How can I occupy the whole map from the beginning? Or maybe another possibility: How can I invert the occupancy map once I created my open path?

Thanks!

Answer 1

[Peter Bauman]

solved the issue the following way:

% Create binaryOccupancyMap
myMap = binaryOccupancyMap(xRange, yRange, 10);

% occupy the path I know will be available
myMap.setOccupancy([path(1,:)' path(2,:)'], ones(length(path), 1))

% inflate the path to create a corridor
myMap.inflate(0.4)

% Invert Map to Clear Path and occupy the rest
myMapInv = ~myMap.getOccupancy
myMap.setOccupancy(myMapInv);

Answer 2

[Cameron Stabile]

Hi Peter,

Glad you found a solution! Just as a reference for you or others, here are a few alternate ways to accomplish this:

%% Set up problem

xRange = 10;

yRange = 5;

res = 2;

%% 1) Construct from occupied matrix

occupiedMatrix = true(yRange*res,xRange*res);

map1 = binaryOccupancyMap(occupiedMatrix,Resolution=res);

%% 2) Construct map, then fill with occupied values

map2 = binaryOccupancyMap(xRange,yRange,Resolution=res);

map2.setOccupancy(mat);

%% 3) Invert an empty map

map3 = binaryOccupancyMap(xRange,yRange,Resolution=res);

map3.setOccupancy(~map3.getOccupancy());

%% 4) Set entire map to scalar value

map4 = binaryOccupancyMap(xRange,yRange,Resolution=res);

map4.setOccupancy(1);

%% 5) Set using block syntax

map5 = binaryOccupancyMap(xRange,yRange,Resolution=res);

botLeftXY = [0 0];

map5.setOccupancy(botLeftXY, occupiedMatrix);

%% Verify all maps are equivalent, and filled

assert(isequal(map1,map2) && isequal(map1,map2) && isequal(map1,map3) && isequal(map1,map4) && isequal(map1,map5));

assert(isequal(size(map1.getOccupancy),map1.GridSize) && all(map1.getOccupancy()==1,'all'));

In terms of efficiency, the 1st and 4th are likely the least expensive options, but all of these should be significantly faster than inflation.

As for your second request (inflating some region around a desired path), your technique - initializing your path as occupied cells, inflating, and then inverting the map - is a good one!

Hope this helps,

Cameron