slam_prof.m

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

% SLAM_PROF
%
% Profiles several SLAM filters on the same problem.
% 
% Usage: 
%
%   [xE, laE, lmE, fp] = tjtslam_prof(p, F[, OPTIONS])
%           
% Inputs:
%   p        - a SLAM problem structure with T time steps
%              (see SLAMPROB)
%   F        - a k-by-1 cell array of SLAM filter structures (such
%              as that generated by KALMAN_SLAM_FILTER,
%              TJT_SLAM_FILTER or FASTSLAM_FILTER)
% 
% Outputs:
%   xE   - a T x k matrix whose (i, j) entry is the error in 
%          the estimate of the robot's XYZ position at time i given
%          by the filter F{k}
%   laE  - an identically-structured matrix containing the average
%          landmark XYZ position estimate error 
%   lmE  - an identically-structured matrix containing the maximum 
%          landmark XYZ position estimate error 
%   fp   - an identically-structured matrix containing the number
%          of floating point operations used to compute the estimates
%
% Options:
%   'visualize' - a 0/1 flag that, if nonzero, indicates that the
%                 results of the experiments should be plotted in
%                 real-time (default: 1)

% 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 [xm_error, lm_avg_error, lm_std_error, lm_max_error, fpops] ...
    = slam_prof(p, F, varargin)

[visualize] = process_options(varargin, ...
			      'visualize', 1);

import javaslam.util.Flops;

T = size(p.om, 2);
h = length(F);

% Initialize the measurements.
xm_error = zeros(T - 1, h);
fpops = zeros(T - 1, h);
lm_max_error = zeros(T - 1, h);
lm_avg_error = zeros(T - 1, h);
lm_std_error = zeros(T - 1, h);

% Set up the plots.
labels = cell(h, 1);
for i=1:h
  labels{i} = F{i}.label;
end

if (visualize)
  pos = get(gcf, 'Position');
  set(gcf, 'Position', [pos(1), pos(2), 2 * pos(3), pos(4)]);
  set(gcf, 'DoubleBuffer', 'on');
  
  subplot(3, 1, 1);
  set(gcf, 'PaperPositionMode', 'auto');
  set(gca, 'FontSize', 12);
  h1 = plot(fpops); 
  legend(labels, 0);
  %xlabel('Time Step');
  ylabel({'Floating-point'; 'Operations'});
  %set(gca, 'XTick', []);
  set(gca, 'XLimMode', 'manual');
  set(gca, 'XLim', [0, p.T]);
  
  subplot(3, 1, 2);
  set(gcf, 'PaperPositionMode', 'auto');
  set(gca, 'FontSize', 12);
  h2 = plot(xm_error);
  %xlabel('Time Step');
  ylabel('Localization Error');
  %set(gca, 'XTick', []);
  set(gca, 'XLimMode', 'manual');
  set(gca, 'XLim', [0, p.T]);
  
  subplot(3, 1, 3);
  set(gcf, 'PaperPositionMode', 'auto');
  set(gca, 'FontSize', 12);
  h3 = plot(lm_avg_error);
  % hold on;
  % h3a = plot(lm_avg_error + lm_std_error, '--');
  % h3b = plot(lm_avg_error - lm_std_error, '--');
  xlabel('Time Step');
  ylabel('Mapping Error');
  set(gca, 'XLimMode', 'manual');
  set(gca, 'XLim', [0, p.T]);

  if 0
    subplot(4, 1, 4);
    set(gcf, 'PaperPositionMode', 'auto');
    set(gca, 'FontSize', 12);
    h4 = plot(lm_max_error);
    xlabel('Time Step');
    ylabel('Maximum Landmark Position Error');
    set(gca, 'XLimMode', 'manual');
    set(gca, 'XLim', [0, p.T]);
  end
end

% Run the filters.
for k=1:(p.T - 1)
  disp(sprintf('Time step %d of %d:', k, p.T - 1));
  for i=1:h
    disp(sprintf('\tUpdating %s...', F{i}.label));
    % Advance the filter.
    Flops.reset;
    feval(F{i}.ffun, F{i}.bs, p.lfun, p.ilfun, ...
	  p.ym{k}, p.yC, p.yid{k}, 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});
    % Compute the number of fpops
    fpops(k, i) = Flops.count;
    % Extract the filter's map.
    [x, lm, id] = feval(F{i}.mfun, F{i}.bs);
    if (length(id) > 0)
      % Align the filter's map to the true map using a rigid transformation.
      [c, R, t] = ralign(lm, p.lm(:, id));
      lm = R * lm + repmat(t, [1, length(id)]);
      x = R * x(1:2) + t;
    end
    % Compute the localization error.
    xm_error(k, i) = norm(x(1:2) - p.path(1:2, k + 1));
    % Compute the landmark errors and some statistics.
    lm_err = sqrt(sum((lm - p.lm(:, id)).^2, 1));
    if (isempty(lm_err))
      lm_err = [0];
    end
    lm_avg_error(k, i) = mean(lm_err);
    lm_std_error(k, i) = std(lm_err);
    lm_max_error(k, i) = max(lm_err);
    if (visualize)
      % Update the plots.
      set(h1(i), 'YData', fpops(:, i));
      set(h2(i), 'YData', xm_error(:, i));
      set(h3(i), 'YData', lm_avg_error(:, i));
      % set(h3a(i), 'YData', lm_avg_error(:, i) + lm_std_error(:, i));
      % set(h3b(i), 'YData', lm_avg_error(:, i) - lm_std_error(:, i));
      if 0 set(h4(i), 'YData', lm_max_error(:, i)); end
    end
  end
  if (visualize)
    drawnow;
  end
  
  % Pause for a second to assist interrupts.
  pause(0.5);
end

return;