2D Occupancy Grid - Cell correspondance with the measured points

Hello,

I was using this function: binaryOccupancyMap

I created an occupancy grid from my measured data ( 3D point cloud, but sliced so only with x and y coordinates, ingnoring z axis).

I had 2 data sets and made a comparison between them (compared each corresponding cell and only kept the differences in a new grid).

Now I'd like to take the occupancy grid cells that are left, and find the real measured points that would be in a given cell.

Since I'm aware that sometimes I don't formulate my questions well, here's an illustration:

There's an associated function grid2world that I though would help me for that, however it seems to convert a grid's cell center into a world coordinate, not give the actual points inside the cells given as input.

Thx for any input,

[Cameron Stabile]

Hi Adrien,

The map classes are a discretized representation of a continuous space, so when you insert your continuous data (pointcloud) into the map, the points are converted to discrete cell values and the map is updated. The map does not, however, retain the continuous data.

If I understood your question, it sounds like you are trying to find the set of points that do or do not overlap using the map as the "diff". One way to do this is checkOccupancy.

Any point found in an occupied cell will return true, and points lying in free cells return false (points lying outside the map bounds may return -1 for unknown). Putting it together could look something like the following:

%% Generate random data
rangefinder = rangeSensor;
truePose = [0 0 pi/4];
trueMap = binaryOccupancyMap(eye(10));

% Generate the scan.
[ranges, angles] = rangefinder(truePose, trueMap);
scan = lidarScan(ranges, angles)

scan = 
  lidarScan with properties:

       Ranges: [258×1 double]
       Angles: [258×1 double]
    Cartesian: [258×2 double]
        Count: 258

% Visualize the scan.
figure
plot(scan);

%% Create two sets of the data
xyA = scan.Cartesian(all(~isnan(scan.Cartesian),2),:);
xyA = xyA+(rand(size(xyA))-.5)*trueMap.Resolution; % Add noise
xyB = xyA;

% Remove sections from both scans
for i = 1:5
    i0 = randi([1 size(xyB,1)-5],1);
    xyB(i0+5,:) = [];

    i0 = randi([1 size(xyA,1)-5],1);
    xyA(i0+5,:) = [];
end

%% Create a map with each set of sensor data
res = 5;
mapA = binaryOccupancyMap("Resolution",res);
mapB = binaryOccupancyMap("Resolution",res);

setOccupancy(mapA,xyA,1);
setOccupancy(mapB,xyB,1);

figure
show(mapA); hold on; plot(xyA(:,1), xyA(:,2),'r.'); title('Map A');

figure
show(mapB); hold on; plot(xyB(:,1), xyB(:,2),'c.'); title('Map B');

%% Show the diff
mapDiff = binaryOccupancyMap(getOccupancy(mapA)~=getOccupancy(mapB),"Resolution",res);

figure
show(mapDiff); hold on; title('Map Diff');

% Non-overlapping points
nonOverlapA = xyA(checkOccupancy(mapDiff,xyA)==1,:);
nonOverlapB = xyB(checkOccupancy(mapDiff,xyB)==1,:);

% Overlapping points
overlapA = xyA(checkOccupancy(mapDiff,xyA)==0,:);
overlapB = xyB(checkOccupancy(mapDiff,xyB)==0,:);

plot(nonOverlapA(:,1), nonOverlapA(:,2),'cx');
plot(nonOverlapB(:,1), nonOverlapB(:,2),'rx');
plot(overlapA(:,1), overlapA(:,2),'g.');
plot(overlapB(:,1), overlapB(:,2),'g.');

legend({'Nonmatching in xyA','Nonmatching in xyB','Matching in both'})

Hope this helps,

Cameron

[Adrien]

Hello Cameron,

Thank you so much for your very complete answer.

Also, didn't know it was possible to have a condition instead of an array indice to select data. I always did it before and saved it into another variable.

Edit: I don't know how to accept your comment as answer, the option doesn't show up, but it does answer my question

[Cameron Stabile]

Hi Adrien,

Glad it was able to help and introduce a new trick!

Saving to a local mask is often the right thing to do, and realistically you may want to do it here since that call to checkOccupancy can be expensive. That said, if the mask is small (or if the compiler can infer that the result doesn't change between uses) then using the inline syntax can be a bit cleaner and potentially costless.

I've updated the record by copying the comment over to the answer section, hopefully it's more visible now.

Best,

Cameron

Answer 1

[Cameron Stabile]

Hi Adrien,

The map classes are a discretized representation of a continuous space, so when you insert your continuous data (pointcloud) into the map, the points are converted to discrete cell values and the map is updated. The map does not, however, retain the continuous data.

If I understood your question, it sounds like you are trying to find the set of points that do or do not overlap using the map as the "diff". One way to do this is checkOccupancy.

Any point found in an occupied cell will return true, and points lying in free cells return false (points lying outside the map bounds may return -1 for unknown). Putting it together could look something like the following:

%% Generate random data
rangefinder = rangeSensor;
truePose = [0 0 pi/4];
trueMap = binaryOccupancyMap(eye(10));

% Generate the scan.
[ranges, angles] = rangefinder(truePose, trueMap);
scan = lidarScan(ranges, angles)

% Visualize the scan.
figure
plot(scan);

%% Create two sets of the data
xyA = scan.Cartesian(all(~isnan(scan.Cartesian),2),:);
xyA = xyA+(rand(size(xyA))-.5)*trueMap.Resolution; % Add noise
xyB = xyA;

% Remove sections from both scans
for i = 1:5
    i0 = randi([1 size(xyB,1)-5],1);
    xyB(i0+5,:) = [];

    i0 = randi([1 size(xyA,1)-5],1);
    xyA(i0+5,:) = [];
end

%% Create a map with each set of sensor data
res = 5;
mapA = binaryOccupancyMap("Resolution",res);
mapB = binaryOccupancyMap("Resolution",res);

setOccupancy(mapA,xyA,1);
setOccupancy(mapB,xyB,1);

figure
show(mapA); hold on; plot(xyA(:,1), xyA(:,2),'r.'); title('Map A');
figure
show(mapB); hold on; plot(xyB(:,1), xyB(:,2),'c.'); title('Map B');

%% Show the diff
mapDiff = binaryOccupancyMap(getOccupancy(mapA)~=getOccupancy(mapB),"Resolution",res);

figure
show(mapDiff); hold on; title('Map Diff');

% Non-overlapping points
nonOverlapA = xyA(checkOccupancy(mapDiff,xyA)==1,:);
nonOverlapB = xyB(checkOccupancy(mapDiff,xyB)==1,:);

% Overlapping points
overlapA = xyA(checkOccupancy(mapDiff,xyA)==0,:);
overlapB = xyB(checkOccupancy(mapDiff,xyB)==0,:);

plot(nonOverlapA(:,1), nonOverlapA(:,2),'cx');
plot(nonOverlapB(:,1), nonOverlapB(:,2),'rx');
plot(overlapA(:,1), overlapA(:,2),'g.');
plot(overlapB(:,1), overlapB(:,2),'g.');

legend({'Nonmatching in xyA','Nonmatching in xyB','Matching in both'})

Hope this helps,

Cameron

[Adrien]

Hello,

I was trying to basically do the same thing in 3D this time.

I first tried your method for getting the diff between 2 maps, however the "getOccupancy" function behaves differently in 3D maps (in 2D one use of it allows as argument only a map, and returns a matrix, but in 3D it is necessary to have 2 arguments, the map and xyz points, and returns an array telling me if at xyz coordinates, the cell is occupied). The way it behaves in 3D then, makes it impossible to compare the getOccupancy results between 2 maps if the initial XYZ vectors of those 2 maps aren't the same size.

So I wanted to use my previous method I tried in 2D, that is to use "occupancyMatrix" to check in a loop (would be slow but I don't know any other way) however there doesn't seem to be an equivalent of it for 3D maps.

Would you have any idea about it?

PS: I hesitated to open a new question but I feel like it's kind of a follow up question