function moments(o,d)
% moments(o,d) demonstrates line fitting using moments (PCA)
%
%   Arg o is the CCW line normal angle in radians (default 0).
%
%   Arg d is the perp distance from the line to the origin (default 0).
%
%   Other config parameters can be changed below.

if (nargin < 2) d = 0; end
if (nargin < 1) o = 0; end

ax = 100; % axis limit
n = 50; % num data points
w = 100; % data width
sigma = 2; % ordinate stdev
t = o + pi/2; % tangent dir
cx = cos(o)*d; cy = sin(o)*d; % data centroid (center pt on line)
tx = cos(t); ty = sin(t); % tangent
nx = -ty; ny = tx; % normal

%x = w*(rand(1,n)-0.5); % random abscissas in standard pose
x = linspace(-w/2, w/2, n); % uniform abscissas in standard pose
y = sigma*randn(1,n); % random ordinates in standard pose

D = [tx nx cx; ty ny cy]*[x; y; ones(1,n)]; % transform to given pose
x = D(1,:); y = D(2,:); x = x(:); y = y(:); % put data in column vectors

fprintf('config n=%f, w=%f, sigma=%f\n', n, w, sigma);
fprintf('given o=%f, t=%f, d=%f, cx=%f, cy=%f\n', o, t, d, cx, cy);

function plotline(cx, cy, t, spec)
  ltx = cos(t); lty = sin(t); % tangent
  lnx = -lty; lny = ltx; % normal
  D = [ltx lnx cx; lty lny cy]*[-w w; 0 0; 1 1];
  plot(D(1,:), D(2,:), spec);
end

clf; hold on; axis([-ax ax -ax ax]); grid on;

%plotline(cx, cy, t, 'b-');
plot(x, y, '.'); % data

% Line fitting code starts here.  At this point the data point coordinates are
% in the nx1 vectors x and y.

% moments
%m00 = n; (zeroth)
m10 = sum(x); m01 = sum(y); m11 = sum(x.*y); % first
m20 = sum(x.*x); m02 = sum(y.*y);  % second

mx = m10/n; my = m01/n; % centroid

% components of scatter matrix
a = m20/n-mx*mx;
c = m02/n-my*my;
b = m11/n-mx*my;

% theoretically, we could construct the scatter matrix S and then select the
% eigenvector corresponding to its larger eigenvalue
%S = [a b; b c];
%[V,D] = eig(S);
%if (D(1,1)>D(2,2)) rt = atan2(V(2,1),V(1,1));
%else rt = atan2(V(2,2),V(1,2)); end

% but this is equivalent and doesn't require calculating an eigendecomposition
rt = 0.5*atan2(2*b,a-c);

fprintf('recovered cx=%f, cy=%f, t=%f (+pi=%f)\n', mx, my, rt, rt+pi);

plot(mx, my, 'r*');
plotline(mx, my, rt, 'r-');

end
