slam_sim.m

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

% SLAM_SIM - Applies a SLAM filter to a SLAM problem.
%
% Usage:
%
%   slam_sim(p, f[, OPTIONS])
%
% Inputs:
%
%   p  - a SLAM problem structure (see SLAMPROB)
%   f  - a SLAM filter structure (such as generated by 
%        KALMAN_SLAM_FILTER, TJT_SLAM_FILTER, or FASTSLAM_FILTER)
% 
% Options:
%
%   'filter-opts' arguments that are passed to the filtering method
%                 of the filter (default: {})
%   'plot-opts'   arguments that are passed to the plotting method
%                 of the filter (default: {})
%   'known-ids'   a 0/1 flag; if non-zero, then the filter is told
%                 of the correct data association (default: 1)
%   'start'       the first time step of the problem to present to
%                 the filter (default: 1)
%   'finish'      the last time step of the problem to present to
%                 the filter (default: p.T)
%   'visualize'   a 0/1 flag; if non-zero, then the simulation is 
%                 plotted (default: 0)
%   'interactive' a 0/1 flag; if non-zero, then the simulation is paused
%                 between updates (default: 0)
%   'filename'    if non-empty, then this string specifies a
%                 filename to which an AVI movie of the 
%                 simulation is written (default: [])
%   'avi-opts'    options that are passed to AVIFILE if a movie is 
%                 created (default: {'Compression', 'none', 'FPS', 12})
%   'resolution'  if non-empty, this must be a two vector 
%                 [height width] giving the resolution
%                 of the figure window (default: [], or
%                 [640 480] if 'filename' is specified)
%

% 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 [] = slam_sim(p, f, varargin)

help slam_sim;

[start, ...
 finish, ...
 visualize, ...
 filename, ...
 interactive, ...
 verbose, ...
 resolution, ...
 known_da, ...
 avi_opts, ...
 filter_opts, ...
 plot_opts] = process_options(varargin, ...
			      'start', 1, ...
			      'finish', [], ...
			      'visualize', 1, ...
			      'filename', [], ...
			      'interactive', 0, ...
			      'verbose', 0, ...
			      'resolution', [], ...
			      'known-ids', 1, ...
			      'avi-opts', {'Compression', 'none', ...
		                           'FPS', 12}, ...
			      'filter-opts', {}, ...
			      'plot-opts', {});

if interactive
  verbose = 1;
end

if (isempty(finish)) 
  finish = p.T; 
end

if (~isempty(filename) | interactive)
  visualize = 1;
end

if (~isempty(filename))
  resolution = [640 480];
  avi = avifile(filename, avi_opts{:});
end

if (interactive)
  pause on;
else
  pause off;
end

% idmap(i) contains the relative index of the landmark most recently
% associated with the landmark whose absolute index is i.
idmap = zeros(2 * size(p.lm, 2), 1);
correct = [];
incorrect = [];

for k=start:finish
  if visualize
    % Plot the belief state and color the landmarks as follows:
    %   green:   a measurement was correctly associated to the landmark
    %   blue:    a measurement was incorrectly associated to the landmark
    %   black:   no measurements were associated to the landmark
    colors = cell(size(p.lm, 2), 1);
    if (~isempty(colors))
      [colors{:}] = deal('b');
    end
    if (~isempty(correct))
      [colors{id(correct)}] = deal('g');
    end
    if (~isempty(incorrect))
      [colors{id(incorrect)}] = deal('r');
    end

    % Plot the current time step.
    slam_plot(p, f, 'time', k, 'resolution', resolution, ...
	      'plot-opts', {plot_opts{:}, 'colors', colors});

    if (~isempty(filename))
      avi = addframe(avi, gcf);
    end
  end
  
    
  if (verbose)
    disp(sprintf('Filtering step %d; please wait...', k));
  end

  % Advance the filter.
  if known_da
    id = p.yid{k};
  else
    % Perform data association.
    id = feval(f.dafun, f.bs, p.lfun, p.ym{k}, p.yC, 'args', p.largs{k});
  end
  feval(f.ffun, f.bs, p.lfun, p.ilfun, p.ym{k}, p.yC, id, p.xfun, ...
	p.G, p.ofun, p.om(:, k), p.oC, 'xargs', p.xargs{k}, ...
	'largs', p.largs{k}, 'oargs', p.oargs{k}, 'ilargs', p.ilargs{k});

  if visualize
    % Compute which measurements were correctly associated.
    prev_id = idmap(p.yid{k});
    correct = find(((prev_id(:) == id(:)) | (prev_id(:) == 0)));
    incorrect = find(((prev_id(:) ~= id(:)) & (prev_id(:) ~= 0)));
    % Update the landmark ID map.
    idmap(p.yid{k}(correct)) = id(correct);
    
    % Pause before plotting until the user advances the time.
    if (interactive)
      disp('Press any key to continue...');
      pause;
    end
  end
end

if visualize
  if (~isempty(filename))
    % Plot the last time step.
    slam_plot(p, f, finish, 'plot-opts', {plot_opts{:}, 'colors', colors}, ...
	      'resolution', resolution);
    for i=1:20
      avi = addframe(avi, gcf);
    end
    avi = close(avi);
  end
  hold off;
end