2D Occupancy Grid - Cell correspondance with the measured points
5 次查看(过去 30 天)
显示 更早的评论
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/971810/image.jpeg)
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,
3 个评论
Cameron Stabile
2022-4-27
编辑:Cameron Stabile
2022-4-27
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
采纳的回答
Cameron Stabile
2022-4-27
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
更多回答(0 个)
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!