www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tools/doMapping.m
function World = doMapping(World, sen, r_new, scan_data, handles) %%%%%%%%%%%%%%%%%%%%%%%%% MAPPING SETTINGS %%%%%%%%%%%%%%%%%%%%%%%%%%%% % Amount by which to increment/reduce cell value when obstacle is detected: map_augment = 20; % Amount by which to reduce cell value when no obstacle is detected: map_reduce = 20; map_max = 255; % Maximum value that a grid cell can take map_min = 0; % Minimum value that a grid cell can take map_isOccupied = 150; % Value at which cell is in occupied state map_isEmpty = 50; % Value at which cell is in empty state %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% scan_polar = getMeasurement(scan_data); % Determine the region in the scan that was detected as being emtpy empty_region_pol = ones(2, size(scan_polar, 2) * ... (round((sen.range - sen.range_min)/World.map_res) + 1)) * -1; position = 1; for i = 1:size(scan_polar, 2) empty_region_r = sen.range_min : World.map_res : scan_polar(1, i) ... - World.map_res; empty_region_a = repmat(scan_polar(2, i), 1, length(empty_region_r)); empty_region_pol(:, position : position + length(empty_region_r) - 1) ... = [empty_region_r; empty_region_a]; position = position + length(empty_region_r); end empty_region_pol = empty_region_pol(:, empty_region_pol(1, :) ~= -1); % Map the empty region of the scan onto the global frame empty_region = invScanPoint(r_new, empty_region_pol); scan_global = transToGlobal(r_new, scan_data); gridmap_occupied = zeros(size(World.gridmap)); gridmap_empty = zeros(size(World.gridmap)); scan_global = interp1(World.map_vals, World.map_vals, scan_global, 'nearest'); empty_region = interp1(World.map_vals, World.map_vals, empty_region, 'nearest'); for i = 1:length(World.map_vals) for j = 1:length(World.map_vals) if ~isempty(scan_global) gridmap_occupied(i, j) = sum(scan_global(1,:) == World.map_vals(j) & ... scan_global(2,:) == World.map_vals(i)); end gridmap_empty(i, j) = sum(empty_region(1,:) == World.map_vals(j) & ... empty_region(2,:) == World.map_vals(i)); end end World.gridmap_counter = World.gridmap_counter ... + map_augment * gridmap_occupied ... - map_reduce * gridmap_empty; % Set the gridmap counter such that the max value is 255 and min value % is 0: World.gridmap_counter(World.gridmap_counter > map_max) = map_max; World.gridmap_counter(World.gridmap_counter < map_min) = map_min; gridmap_occupied = World.gridmap_counter > map_isOccupied; gridmap_empty = World.gridmap_counter < map_isEmpty; World.gridmap = ones(size(World.gridmap_counter)) * 0.5; World.gridmap(gridmap_occupied) = 1; World.gridmap(gridmap_empty) = 0; World.gridmap_greyscale = mat2gray(World.gridmap_counter, [map_min map_max]); imagesc(flipud(World.gridmap_greyscale), 'parent', handles.map1, [0 1]); set(handles.map1, 'XTick', [], 'YTick', []); imagesc(flipud(World.gridmap), 'parent', handles.map2, [0 1]); set(handles.map2, 'XTick', [], 'YTick', []); end