robotics專項課Estimation and Learning assignment 3: Occupancy Grid Mapping
阿新 • • 發佈:2019-02-14
% 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