function [x,P, xs] = align_least_squares(x0, z, R, idx, nit)
%function [x,P, xs] = align_least_squares(x0, z, R, idx, nit)
%
% INPUTS:
%   x0 - set of M initial states (global poses), a 3M state vector
%   z - set of N observations (relative poses), a 3N observation vector
%   R - uncertainty of each observe, a 3N x 3N matrix
%   idx - index of each observe into state vector, (2 x N), [i; j]
%   nit - number of non-linear least squares iterations
%
% OUTPUTS:
%   x - final M pose estimates
%   P - covariance of pose estimates
%   xs - stored estimates for each iteration of least-squares algorithm, (3M x nit) matrix
%
% Tim Bailey 2004.

Ri = inv(R);
x = x0;

% perform iterated least-squares
for i=1:nit
    tic
    [v, H] = make_jacobian(x, z, idx);    

    P = inv(H'*Ri*H);
    dx = P*H'*Ri*v;    
    x = x + dx;
    xs(:,i) = x;
    toc
end

%
%

function [v,H] = make_jacobian(x, z, idx)
% Compute innovation and Jacobian for set of observations, z. 
% The first (implicit) observation is location of first scan.

lenx = length(x);
lenz = length(z);

H = zeros(lenz, lenx);
v = zeros(lenz, 1);

% First observation: the location of first pose is known exactly: z(1:3)
ii = 1:3;
H(ii,ii) = [1 0 0; 0 1 0; 0 0 1];
v(ii) = z(ii) - x(ii);
v(3) = pi_to_pi(v(3));

% All other observations: the relative pose between pose-pairs.
ctr = 4;
for i=1:size(idx,2)
    i1 = get_index(idx(1,i)); % index pose 1
    i2 = get_index(idx(2,i)); % index pose 2
    iz = ctr:(ctr+2);         % index observation: pose 2 wrt pose 1
    ctr = ctr+3;
    
    [v(iz), H1, H2] = compute_jacobian(x(i1), x(i2), z(iz));
        
    H(iz, i1) = H1;
    H(iz, i2) = H2;
end

%
%

function [v,H1,H2] = compute_jacobian(x1, x2, z)
% Compute the innovation and Jacobian for a single relative pose observation

c = cos(x1(3));
s = sin(x1(3));

% Innovation (ie, residual between z and predicted z)
R = [c s 0;
    -s c 0;
     0 0 1];
 
zpred = R * (x2 - x1); % our observation model
v = z - zpred;
v(3) = pi_to_pi(v(3));

% Jacobian wrt x1
H1 = [-c -s -(x2(1)-x1(1))*s + (x2(2)-x1(2))*c;
       s -c -(x2(1)-x1(1))*c - (x2(2)-x1(2))*s;
       0  0 -1];

% Jacobian wrt x2
H2 = [c s 0;
     -s c 0
      0 0 1];

%
%

function ii = get_index(i)
% Get index set for pose i in the state vector. The 3 indices are for xi, yi, phii, respectively.
ii = 3*i - 2;
ii = ii:(ii+2);
