calccompactness.m

用于移动机器人SLAM的仿真工具箱（CAS Robot Navigation Toolbox）。由Kai Arras编写。可迅速构建移动机器人仿真平台。

%CALCCOMPACTNESS Calculate relative model fidelity measure.
%   C = CALCCOMPACTNESS(X) returns the sum of Mahalanobis distances
%   of n alpha,r-lines from their common weighted mean in the alpha,
%   r-model space. X is of dimension 5xn, where the rows are [alpha,
%   r, saa, srr, sar]. The coordinate alpha is properly unwrapped.
%
%   If the covariance matrices are set to identity, the function re-
%   turns the sum of the squared Euclidian distances from the common
%   arithmetic mean.
%
%   See also EXTRACTLINES.

% v.1.0, 14.08.97, Kai Arras, ASL-EPFL
% v.1.1, 14.01.99, Kai Arras, ASL-EPFL
% v.1.2, Dec.2003, Kai Arras, CAS-KTH: toolbox version

function c = calccompactness(x);

m = size(x,1);
n = size(x,2);
if m == 5,
  
  % Step 1: transform alphas to eliminate cyclic alpha-axis.
  % The values are shifted such that their minimum lies on zero.
  minalpha = min(x(1,:));
  for i = 1:n,
    diffi = x(1,i)-minalpha;
    if diffi > pi,
      x(1,i) = diffi - 2*pi;
    else
      x(1,i) = diffi;
    end;
  end;
    
  % Step 2: stack and apply multivariate mean
  xv = zeros(2,n);
  Cv = zeros(2,2,n);
  for i = 1:n,
    xv(:,i)   = [x(1,i); x(2,i)];
    Cv(:,:,i) = [x(3,i), x(5,i); x(5,i), x(4,i)];
  end;
  [xw,Cw] = meanwm(xv,Cv);
  
  % Step 3: sum all mahalanobis distances
  accu = 0; 
  for i = 1:n,
    l2 = [xw(1) xw(2) 0 0 0];
    accu = accu + mahalanobisar(x(:,i),l2);
  end;
  c = accu;
  
else
  disp('calccompactness: Wrong input. Check your arguments.'); c = -1;
end;