www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tools/doGraphics.m
% Process and plot all of the loop graphics. function doGraphics(rob, World, Graphics, Obstacles_mov, AxisDim) if ~isempty(World.scan_global) set(Graphics.Scan, 'xdata', World.scan_global(1,:), 'ydata', World.scan_global(2,:)) else set(Graphics.Scan, 'xdata', [], 'ydata', []) end % Draw robot poses TrueRobTri = rob.computeTriangle('true'); RobTri = rob.computeTriangle; set(Graphics.TrueRob, 'xdata', TrueRobTri(1, :), 'ydata', TrueRobTri(2, :)); set(Graphics.Rob, 'xdata', RobTri(1, :), 'ydata', RobTri(2, :)); % Draw landmarks lids = find(World.l(1,:)); lx = World.x(World.l(1,lids)); ly = World.x(World.l(2,lids)); set(Graphics.Lmk, 'xdata', lx, 'ydata', ly); % Draw landmark covariance ellipses for lid = lids le = World.x(World.l(:,lid)); LE = World.P(World.l(:,lid),World.l(:,lid)); [X,Y] = cov2elli(le,LE,3,16); set(Graphics.Ellipse(lid),'xdata',X,'ydata',Y); end % Draw robot position covariance ellipse if World.t > 1 re = World.x(World.r(1:2)); RE = World.P(World.r(1:2),World.r(1:2)); [X,Y] = cov2elli(re,RE,3,16); set(Graphics.RobCov,'xdata',X,'ydata',Y); end % Draw landmark sensor lines if ~isempty(Graphics.lmks_visible) vis_lmks_loci = zeros(2, 2 * length(Graphics.lmks_visible)); for i = 1 : length(Graphics.lmks_visible) k = Graphics.lmks_visible(i); vis_lmks_loci(:, 2*i-1) = rob.R(1:2); %vis_lmks_loci(:, 2*i) = World.W(:, k); vis_lmks_loci(:, 2*i) = invScanPoint(rob.R, Graphics.lmks_all(:, k)); end set(Graphics.LmkLines, 'xdata', vis_lmks_loci(1, :), 'ydata', vis_lmks_loci(2, :)); % Reset lmk lines else set(Graphics.LmkLines, 'xdata', [], 'ydata', []); end % Plot moving obstacles for i = 1:length(Obstacles_mov) Obstacles_mov(i).plot; end axis([-AxisDim AxisDim -AxisDim AxisDim]) drawnow; end