function BundleAdjustment1D

nPoints = 5;
nCameras = 3;
X = rand(nPoints, 2) + 2;
c = rand(nCameras,2);

X0 = X;
c0 = c;

figure(2);
clf;
plot(X(:,1), X(:,2), 'b.');
for i = 1 : nCameras
    m = X-ones(nPoints,1)*c(i,:);
    theta = atan2(m(:,2), m(:,1));
    theta = mean(theta);
    
    C(i).c = c(i,:)';
    theta = theta;
    C(i).R = [sin(theta) -cos(theta); cos(theta) sin(theta)];   
    
    u = C(i).R*[eye(2) -C(i).c] * [X'; ones(1,nPoints)];
    C(i).m = u(1,:)./u(2,:);
    
    C(i).c = c(i,:)'+0.3*randn(2,1);
    theta = theta + 0.3*randn(1,1);
    C(i).R = [sin(theta) -cos(theta); cos(theta) sin(theta)];   
    
    
    hold on
    plot(C(i).c(1), C(i).c(2), 'ko');
    hold on
    plot([C(i).c(1) C(i).c(1)+C(i).R(2,1)], [C(i).c(2) C(i).c(2)+C(i).R(2,2)], 'k-');
end
X = X + 0.3*randn(size(X));
axis equal

[C X] = BA(C, X);

X0-X
for i = 1 : nCameras
    norm(c0(i,:)-C(i).c')
end



function [C X] = BA(C, X)

lambda = 0.5;
nIters = 100;

xp = [];
for i = 1 : length(C)
    theta=atan2(C(i).R(2,2), C(i).R(2,1));
    xp = [xp; C(i).c; theta];
end

xx = [];
for i = 1 : size(X,1)
    xx = [xx; X(i,:)'];
end
error = [];
for j = 1 : nIters  
    Jp = [];
    Jx = [];
    D_inv = [];
    err = [];
    for iPoint = 1 : size(X,1)
        X1 = xx(2*(iPoint-1)+1:2*iPoint);
        df_dX = [];
        d = zeros(2,2);
        for iC = 1 : length(C)
            c = xp(3*(iC-1)+1:3*(iC-1)+2);
            theta = xp(3*(iC-1)+3);
            
            R = [sin(theta) -cos(theta); cos(theta) sin(theta)];
                    
            df_dc = JacobianC1D(R, c, X1);
            df_dR = JacobianR1D(R, c, X1)*JacobianQ1D(theta);
            df_dx = JacobianX1D(R, c, X1);
                    
            j1 = zeros(1,3*length(C));
            j2 = zeros(1,2*size(X,1));
            
            j1(:,3*(iC-1)+1:3*iC) = [df_dc df_dR];            
            j2(:,2*(iPoint-1)+1:2*iPoint) = df_dx;
            
            Jp = [Jp; j1];
            Jx = [Jx; j2];
            
            d = d + df_dx'*df_dx;
            
            u = R * [eye(2) -c] * [X1;1];
            u = u/u(2);
            
            u1 = C(iC).m;

            e = [u1(iPoint) - u(1)];
            err = [err; e];
        end
        d = d + lambda*eye(2);
        D_inv = blkdiag(D_inv, inv(d));
    end
    error(end+1) = norm(err);
    ep = Jp' * err;
    ex = Jx' * err;
    A = Jp'*Jp + lambda*eye(3*length(C));
    B = Jp'*Jx;
    delta_p = inv(A-B*D_inv*B') * (ep-B*D_inv*ex);
    delta_x = D_inv * (ex-B'*delta_p);
    
    xp = xp + delta_p;
    xx = xx + delta_x;
end

figure(1)
clf;
plot(error);

for iC = 1 : length(C)
    C(iC).c = xp(3*(iC-1)+1:3*(iC-1)+2);
    theta = xp(3*(iC-1)+3);        
    C(iC).R = [sin(theta) -cos(theta); cos(theta) sin(theta)];
end    

for iX = 1 : size(X,1)
    X(iX,:) = xx(2*(iX-1)+1:2*iX);
end


function df_dc = JacobianC1D(R, C, X)

x = R * (X-C);

u = x(1);
w = x(2);

del = -R;
du_dc = del(1,:);
dw_dc = del(2,:);

df_dc = [(w*du_dc-u*dw_dc)/w^2];

function df_dx = JacobianX1D(R, C, X)

x = R * (X-C);

u = x(1);
w = x(2);

del = R;
du_dc = del(1,:);
dw_dc = del(2,:);

df_dx = [(w*du_dc-u*dw_dc)/w^2];

function df_dR = JacobianR1D(R, C, X)

x = R * (X-C);

u = x(1);
w = x(2);

du_dc = [(X-C)' zeros(1,2)];
dw_dc = [zeros(1,2) (X-C)'];

df_dR = [(w*du_dc-u*dw_dc)/w^2];

function dR_dq = JacobianQ1D(theta)
dR_dq = [cos(theta) sin(theta) -sin(theta) cos(theta)]';



