% Robotics: Estimation and Learning
% WEEK 3
%
% Complete this function following the instruction.
function myMap = occGridMapping(ranges, scanAngles, pose, param)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
% Parameters
%
% the number of grids for 1 meter.
myResol = param.resol;
% the initial map size in pixels
myMap = zeros(param.size);
% the origin of the map in pixels
myorigin = param.origin;
% 4. Log-odd parameters
lo_occ = param.lo_occ;
lo_free = param.lo_free;
lo_max = param.lo_max;
lo_min = param.lo_min;
N = size(pose,2);
M = size(scanAngles);
for j = 1:N % for each time,
% Find grids hit by the rays (in the gird map coordinate)
iStart = [ceil(pose(1,j)*myResol);ceil(pose(2,j)*myResol)] + myorigin;
for i = 1:M
beta = pose(3,j) +scanAngles(i);
d = ranges(i,j);
pOcc = [d*cos(beta)+pose(1,j); -d*sin(beta)+pose(2,j)];
% Find occupied-measurement cells and free-measurement cells
iOcc = ceil(pOcc*myResol)+myorigin;
[freex, freey] = bresenham(iStart(1),iStart(2),iOcc(1),iOcc(2));
occInd = sub2ind(param.size, iOcc(2), iOcc(1));
freeInd = sub2ind(param.size, freey, freex);
% Update the log-odds
myMap(freeInd) = myMap(freeInd) - lo_free;
myMap(occInd) = myMap(occInd) + lo_occ;
% Saturate the log-odd values
end
% Visualize the map as needed
end
end
robotics专项课Estimation and Learning assignment 3: Occupancy Grid Mapping
猜你喜欢
转载自blog.csdn.net/xiaoshuying/article/details/80165436
今日推荐
周排行