function demo_least_squares()
% This demo shows the batch constraint of a set of relative pose estimates (such as
% those obtained from scan matching). The idea is that, given a set of relative pose
% measurements between scans, we estimate the global pose of each scan to minimise
% the total relative pose errors.
%
% Note, this implementation is written for simplicity not efficiency. It is quite slow, so be patient.
%
% Tim Bailey 2004.

load data % load r, x0, z, R, idx
% r is a set of laser scans
% x0 is an initial set of pose estimates for each scan
% z is a set of relative pose observations between scan-pairs
% R is the uncertainty matrix for each z
% idx is an index, idx(:,i) == [a;b], says that z(i) is the relative pose between scans a and b.

plot_laser(r, x0)
title('Map before least-squares alignment')

[x0, z, R] = reshape_data(x0, z, R);
[x, P, xs] = align_least_squares(x0, z, R, idx, 3);
[x, P] = reshape_results(x, P);

plot_laser(r, x)
title('Map after least-squares alignment')

%
%

function [x, z, R] = reshape_data(x, z, R)
% Convert data into appropriate state-vector form

M = size(x,2);
N = size(z,2);
TINY = 1e-9;

% concatenate x, z and R
x = reshape(x, 3*M, 1);
z = reshape(z, 3*N, 1);

z = [0;0;0;z]; % add absolute first observe
C = eye(length(z))*TINY;
ctr = 4;
for i=1:N
    ii = ctr:ctr+2;
    C(ii,ii) = R(:,:,i);
    ctr = ctr + 3;
end
R = C;

%
%

function [x, P] = reshape_results(x, P)
% Convert data from state vector back into individual set of poses

M = length(x)/3;
x = reshape(x, 3, M);
C = zeros(3,3,M);
ctr = 1;
for i=1:M
    ii = ctr:ctr+2;
    C(:,:,i) = P(ii,ii); % keep block diagonals of P (ie, the marginal pose covariances)
    ctr = ctr + 3;
end
P = C;

%
%

function plot_laser(r, x)
p = get_points(r(1,:), x(:,1));
figure, plot(p(1,:),p(2,:), '.')
hold on, grid on, axis equal
for i=2:size(x,2)
    p = get_points(r(i,:), x(:,i));
    plot(p(1,:),p(2,:), '.', x(1,i), x(2,i), 'x')
end
drawnow

%
%

function p = get_points(r, x)
MAXR = 50;
phi = -pi/2:(pi/360):pi/2;
ii = find(r < MAXR);
r = r(ii);
phi = phi(ii);
p = [r.*cos(phi); r.*sin(phi)];
p = transform_to_global(p, x);
