close all; clear;
path(path, '../oldhouse2');
% with the affine tracker and undone radial distortion
seq_name = 'oldhouse2/A2000';
image_type = 'bmp';
load oldhouse2/A2000_result_ST;  % tracks from 110 to 200

% specify index of starting frame fs, end frame fe
% and the subsampling factor ft
fs = 0; fe = 88; ft = 12;
findex = [fs:ft:fe]; offset = 1;
indf = 1:size(findex,2);

% find features tracked in all the frames
ind = find(goodfeat ~= 0);
j = 0;
for i = (1+offset-1):ft:(fe-fs+1)
    j = j+1;
    xim(1,:,j) = xttfirst(2,ind) + SaveSTB(2,ind,i);
    xim(2,:,j) = xttfirst(1,ind) + SaveSTB(1,ind,i);
    xim(3,:,j) = 1;
end;

% consider only indf frames
xim = xim(:,:,indf);
[s, n, m] = size(xim);

opt = '%03d';
imfile = sprintf('%s%s.%s',seq_name,sprintf(opt,findex(1)),image_type)
seq(1).im = (imread(imfile));

% read images
for i = 2:m
    if findex(1) < 10 opt = '%03d';
    elseif findex(1) < 100 opt = '%02d';
    else  opt = '%01d';
    end;
    imfile = sprintf('%s%s.%s',seq_name,sprintf(opt,findex(i)),image_type)
    seq(i).im = imread(imfile);
end;


[ydim,xdim,cdim] = size(seq(1).im);

%-------------------------------------------------------------
% Structure and motion and focal length recovery given xim

% guess intrinsic parameter matrix
[s, n, m] = size(xim);
fguess = max(xdim,ydim);
Aguess = [fguess 0 xdim/2; 0 fguess ydim/2; 0 0 1];

%----------------------------
% Normalize the measurements
for i = 1:m
    xn(:,:,i) = inv(Aguess)*xim(:,:,i);   % partially calibrated views
end

%--------------------------------------------
% uncalibrated case - two view initialization
% compute F and decompose it
Rhat(:,:,1) = diag([1 1 1]);
That(:,1) = zeros(3,1);
F = dfundamental(xn(:,:,1), xn(:,:,m))
[uf, sf, vf] = svd(F'); ep = vf(:,3);
M = skew(ep)'*F; %  + rand(3,1)*ep';
Rhat(:,:,m) = M;
That(:,m) = ep;

%----------------------------
% compute projective stucture
[X,lambda] = compute3DStructure(xn(:,:,1),xn(:,:,m),Rhat(:,:,m),That(:,m));
alpha = 1./lambda;
alpha = alpha/alpha(1);

%------------------------------------------------
% Compute initial motion estimates for all frames
init_error = 10; fs = [100]; ns = [];
errabs = init_error; err_prev = 500; iter = 1; errrel = init_error;

while (errabs > 1e-4) &  (iter < 30)  &  (errrel > 1e-5) % (errlambda > 1e-4)
    for k = 2:m
        j = indf(k);
        Q = []; %  setup matrix P
        for i = 1:n
            Q = [Q; kron(skew(xn(:,i,j)),xn(:,i,1)') alpha(i)*skew(xn(:,i,j))];
        end;
        [um, sm, vm] = svd(Q);
        That(:,j) = vm(10:12,12);
        Rhat(:,:,j) = reshape(vm(1:9,12),3,3)';
    end;
    %--------------------------------------
    % recompute alpha's based on all views
    lambda_prev = lambda;
    % recompute alpha's
    for i = 1:n
        M = [];  % setup matrix M
        for k = 2:m        % set up Hl matrix for all m views
            j = indf(k);
            a = [ skew(xn(:,i,j))*That(:,j) skew(xn(:,i,j))*Rhat(:,:,j)*xn(:,i,1)];
            M = [M; a];
        end;
        alpha(i) = -M(:,1)'*M(:,2)/norm(M(:,1))^2;
    end;
    scale = alpha(1);
    alpha = alpha/scale; % set the global scale
    lambda = 1./alpha;
    X = [lambda.*xn(1,:,1); lambda.*xn(2,:,1); lambda.*xn(3,:,1); ones(1,n)];
    res = [];
    i = 1;
    for l = 1:m
        j = indf(l);
        P(i*3-2:i*3,:) = [Rhat(:,:,j) scale*That(:,j)];
        tt = P(i*3-2:i*3,:)*X;
        if sum(sign(tt(3,:))) < -n/2
            P(i*3-2:i*3,:) = -[Rhat(:,:,j) scale*That(:,j)];
            Rhat(:,:,j) = -Rhat(:,:,j);
            That(:,j) = -That(:,j);
            tt = P(i*3-2:i*3,:)*X;
        end;
        xr(:,:,i) = Aguess*project(tt);
        xd = xim(1:2,:,j) - xr(1:2,:,i);
        errnorm = sqrt(xd(1,:).^2 + xd(2,:).^2);
        res = [res, errnorm];
        i = i+1;
    end

    f = sum(res)/(n*m);
    iter = iter + 1;
    fs = [fs f];
    if iter > 1
        errrel = norm(fs(iter-1) - f);
        if fs(iter - 1) < f
            errrel = 1e-10;
        end;
    end;
    errabs = norm(f);
    errlambda = norm(lambda_prev-lambda);
    lambda_prev = lambda;
end % end while iter

clear xres;
res = [];
%-------------------
% final reprojection
for i = 1:m
    j = indf(i);
    XP(:,:,i) = P(i*3-2:i*3,:)*X;
    xres(:,:,i)  = Aguess*project(XP(:,:,i));
    PC(:,:,i) = [Rhat(:,:,i) That(:,i)];
    xd = xim(1:2,:,j) - xres(1:2,:,i);
    errnorm = sqrt(xd(1,:).^2 + xd(2,:).^2);
    res(i) = sum(errnorm)/(m*n);
end

% self-calibration - linear algorithm for unknown focal length
% set up the constraints on absolute quadric

Omega = quadric_linear_f(PC);
if Omega(1,1) < 0;
    Omega = - Omega
end

for k = 1:m
    t = PC(:,:,k)*Omega*PC(:,:,k)';
    fest(k) = sqrt(t(1,1)/t(3,3));
    fest2(k) = sqrt(t(2,2)/t(3,3));
    Atmp(:,:,k) = Aguess*[fest(k) 0 0; 0 fest(k) 0; 0 0 1];
end

%-------------------------------------------------------
% Estimate the projective to euclidean upgrade transf Hp
for i = 1:m
    v = -[Omega(1,4)/(fest(i)^2); Omega(2,4)/(fest2(i)^2); Omega(3,4)];
    K1 = [fest(i) 0 0; 0 fest2(i) 0 ; 0 0 1];
    v4 = 1;
    Hp(:,:,i) = [K1 zeros(3,1);  -v'*K1  v4];
end
%------------------------------------
% update the structure to Euclidean

Xe = inv(Hp(:,:,1))*X;
Xe = [Xe(1,:,1)./Xe(4,:,1);Xe(2,:,1)./Xe(4,:,1);Xe(3,:,1)./Xe(4,:,1);ones(1,size(X,2))];
if (sum(sign(Xe(3,:,1))) < 0) & (sum(sign(Xe(3,:,1))) == -size(Xe,2))
    Hp = [K1 zeros(3,1);  -v'*K1  -v4];
    Xe = inv(Hp)*X;
    warning('Flipping the sign of Hp');
elseif (sum(sign(Xe(3,:,1))) < 0) & (sum(sign(Xe(3,:,1))) ~= -size(Xe,2))
    error('Projective upgrade: some of the depths are negative');
end;

%-------------------------------
% update the projection matrices
PP = P*Hp(:,:,1);

%------------------------------------------------------
% rigid body motion and 3D Euclidean structure recovery

for i = 1:m
    k = indf(i);
    Re(:,:,i) =  PP(i*3-2:i*3,1:3);
    [r,q] = rq(Re(:,:,i));
    Arem(:,:,i) = r; % /r(3,3); % calibration matrix
    Re(:,:,i) = q; % rotation matrix
    Te(:,i) =  inv(Arem(:,:,i))*PP(i*3-2:i*3,4);
    Xe(:,:,i) = [Re(:,:,i) Te(:,i); 0 0 0 1]*[Xe(:,:,1)];
end;

%------------------------------
% plot euclidean reconstruction
figure;
gs = 100; % global scale
plot3(gs*Xe(1,:,1),gs*Xe(2,:,1),gs*Xe(3,:,1),'k.');  hold on; box on;
xlabel('x'); ylabel('y'); zlabel('z');
draw_scale = gs*Xe(3,1,1)/6;
for i=1:size(Xe,2)
    text(gs*Xe(1,i,1)+2, gs*Xe(2,i,1)+2, gs*Xe(3,i,1), num2str(i));
end;
title('Euclidean reconstruction from multiple views');
view(220,20); grid off; axis equal; %  axis off;
for i=1:4:m
    draw_frame_scaled([Re(:,:,i)' gs*(-Re(:,:,i)'*Te(:,i))], draw_scale);
end
axis equal; box on; % view(-8,-60);

disp('Euclidean reconstruction complete - press ENTER');

pnum = 3;

xdep = zeros(4, );
RP = P(i*3-2:i*3,:);
RH = Hp(:,:,1);

data = Xe(1:3, :, i);
r =  1;
dmp = datamap(data, r);
[lamkm, Ikm, errorkm] = kmiso(data', pnum, dmp);

pmean1 = mean(xim(:,find(Ikm == 1)), 2);
pmean2 = mean(xim(:,find(Ikm == 2)), 2);
pmean3 = mean(xim(:,find(Ikm == 3)), 2);

pmean = [pmean1 pmean2 pmean3];
[value,IX] = sort(pmean, 2);
seg1 = floor((value(1, 1)+value(1, 2))/2)-20;
seg2 = floor((value(1, 2)+value(1, 3))/2)-20;

%Plane 1 includes feature number 18, 20, 72
plist1 = find(Ikm == IX(1, 1));
A = Xe(1:3, plist1, i)';
b = ones(size(A, 1), 1);
p = A\b;
startx = 1;
endx = 480;
starty = 1;
endy = seg1;
xdep = DepthComp(xreg, seq(i).im, startx, endx, starty, ...
    endy, p, RP, RH);

%Plane 2 includes feature number 32, 64, 69
plist2 = find(Ikm == IX(1, 2));
A = Xe(1:3, plist2, i)';
b = ones(size(A, 1), 1);
p = A\b;
startx = 1;
endx = 480;
starty = seg1;
endy = seg2;

xdep = DepthComp(xreg, seq(i).im, startx, endx, starty, ...
    endy, p, RP, RH);

%Plane 3 includes feature number 37, 67, 70
plist3 = find(Ikm == IX(1, 3));
A = Xe(1:3, plist3, i)';
b = ones(size(A, 1), 1);
p = A\b;
startx = 1;
endx = 480;
starty = seg2;
endy = xdim;

xdep = DepthComp(xreg, seq(i).im, startx, endx, starty, ...
    endy, p, RP, RH);