slam2dplot.m

These Matlab files are a convenient interface for the Java library containing the implementation of

% SLAM2DPLOT
%
% Visualizes a 2D SLAM problem.
%
% The visualization consists of:
%
% - black dots representing landmarks
% -
%
% Usage:
%
%   slam2dplot(p)
%
% Inputs:
%
%   p  - a 2D SLAM problem (such as generated by SLAM2DPROB)
% 
% Options:
%

% Copyright (C) 2002 Mark A. Paskin
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
% USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [] = slam2dplot(p, varargin)

help slam2dplot;

pack;

bottom = min([p.lm, p.path(1:2, :)], [], 2);
top = max([p.lm, p.path(1:2, :)], [], 2);
spread = (top - bottom);
top = top + spread * 0.25;
bottom = bottom - spread * 0.25;

% Clear the figure and bring it forward.
cla;
xlim([bottom(1) top(1)]);
ylim([bottom(2) top(2)]);
set(gca, 'DataAspectRatio', [1 1 1]);
set(gca, 'Units', 'normalized');
set(gca, 'Position', [0, 0, 1, 1]);
set(gca, 'Visible', 'off');
set(gcf, 'Color', [1 1 1]);
%set(gca, 'FontSize', 12);
hold on;

% The integrated odometry.
odo_int = zeros(size(p.path));
odo_int(1:3, 1) = p.path(1:3, 1);
% The integrated control.
ctrl_int = zeros(size(p.path));
ctrl_int(:, 1) = p.path(:, 1);

for k=2:p.T
  % Update the integrated odometry.  Since the translational and
  % rotational velocity are zero at the previous time step (as
  % always), we can treat the odometry as the actual velocity.
  odo_int(:, k) = feval(p.xfun, ...
			[odo_int(1:3, k - 1); p.om(:, k - 1)], ...
			zeros(size(p.G, 1), 1), [0; 0]);
  % Update the integrated control.  
  ctrl_int(:, k) = feval(p.xfun, ctrl_int(:, k - 1), ...
			 zeros(size(p.G, 1), 1), p.xargs{k - 1}{1});

  % Plot the landmark measurements
  nobs = length(p.yid{k});
  if (nobs > 0)
    loc = feval(p.ilfun, repmat(p.path(:, k), 1, nobs), ...
		zeros(3, nobs), p.ym{k});
    plot(loc(1, :), loc(2, :), 'c.', 'MarkerSize', 6);
  end
end

% Plot the true landmarks' locations.
if (~isempty(p.lm))
  plot(p.lm(1, :), p.lm(2, :), 'k.');
end
  
% Plot the robot's true path.
plot(p.path(1, :), p.path(2, :), 'b-', 'LineWidth', 1);

% Plot the integrated odometry and control.
plot(odo_int(1, :), odo_int(2, :), 'r--', 'LineWidth', 1);
plot(ctrl_int(1, :), ctrl_int(2, :), 'g-.', 'LineWidth', 1);

drawnow;

hold off;