% Code for computing the fundamental matrix given a collection of
% inter-frame homographies.
% Details can be found in DICTA 2015 paper:
%       http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=7371221&tag=1

% Any questions are welcomed to yi.zhou@anu.edu.au
% Yi Zhou, Feb 12, 2016

% Code provided here are for personal, research use only;
% copyright and all rights therein are retained by authors and/or other copyright holders.

function [F,e2,valid] = NewMH2F(Hs,format)
% input:
%       Hs: a 9*N matrix, each column is homography in a vector form, i.e. Hs =
%       [Hv1,Hv2,...,HvN], where Hvi is a 9*1 vector, i (- [1,N]
%       format: 1 means the homography is stored in a row major order; 
%               0 means the homography is stored in a column major order;
%               i.e. if
%               H = [h11,h12,h13;...
%                    h21,h22,h23;...
%                    h31,h32,h33]
%               format 1 means Hv = [h11;h12;h13;h21;h22;h23;h31;h32;h33];
%               format 0 means Hv = [h11;h21;h31;h12;h22;h32;h13;h23;h33];
% output:
%       F: the Fundamental matrix
%       e2: the epipole on the second view
%       valid: a flag showing the F is found or not
A1 = [];
A2 = [];
A3 = [];
Hnum = size(Hs,2);

% construct least squares problem
for i = 1:Hnum
    if format == 1
        Hmat = reshape(Hs(:,i),[3,3]);
    else
        Hmat = reshape(Hs(:,i),[3,3])';
    end
    h1 = Hmat(:,1);
    h2 = Hmat(:,2);
    h3 = Hmat(:,3);
    
    A1 = [A1;h1'];
    A2 = [A2;h2'];
    A3 = [A3;h3'];
end

%rank2 approximation
A1 = RankApproximation(A1,2);
A2 = RankApproximation(A2,2);
A3 = RankApproximation(A3,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For debug
% r = rank(A3);
% if r == 3
%     a = 1;
% end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%extend A by adding an additional column I
A1 = [A1 ones(size(A1,1),1)];
A2 = [A2 ones(size(A2,1),1)];
A3 = [A3 ones(size(A3,1),1)];

%normalization 
Q1 = NormLinearTran(A1);
A1 = A1*Q1;
Q2 = NormLinearTran(A2);
A2 = A2*Q2;
Q3 = NormLinearTran(A3);
A3 = A3*Q3;
if rank(A3) == 4
    [U1,D1,V1] = svd(A3);
    D1(:,4) = zeros(size(D1,1),1);
    A3 = U1*D1*V1';
end


%valid check
tf1 = isnan(A1);
tf2 = isnan(A2);
tf3 = isnan(A3);
if sum(tf1) + sum(tf2) + sum(tf3) > 0
    F = [];
    e2 = [];
    valid = 0;
    return;
end
valid = 1;

%solve Least-Squared problem 1
f1 = SolveLS(A1);
f2 = SolveLS(A2);
f3 = SolveLS(A3);

%de-normlization
f1 = Q1*f1;
f1 = f1(1:3);
f2 = Q2*f2;
f2 = f2(1:3);
f3 = Q3*f3;
f3 = f3(1:3);

%solve Least-Squared problem 2 (relative scale)
A_lambda = zeros(3*Hnum,3);
for i = 1:Hnum
    h1 = Hs(1:3,i);
    h2 = Hs(4:6,i);
    h3 = Hs(7:9,i);
    A_lambda(3*i - 2,:) = [f1'*h2 f2'*h1 0];
    A_lambda(3*i - 1,:) = [f1'*h3 0 f3'*h1];
    A_lambda(3*i,:) = [0 f2'*h3 f3'*h2];
end
[U,D,V] = svd(A_lambda);
%%%%%%%%%%%%%%%%%%%%%
D(3,3) = 0;
A_lambda = U*D*V';
[U,D,V] = svd(A_lambda);
%%%%%%%%%%%%%%%%%%%%%
lambda = V(:,end);

%
f1 = lambda(1)*f1;
f2 = lambda(2)*f2;
f3 = lambda(3)*f3;

%
F = [f1 f2 f3];
F = F / norm(F);
[U,D,V] = svd(F);
D(:,3) = zeros(3,1);
F = U*D*V';

%
e2 = null(F');
e2 = h2e(e2);
end

%normalization, a linear transformation
%Input:
%M_(m*3) = [h_1_i' 1
%           h_2_i' 1
%             ...
%           h_m_i' 1]
%h_m_i means the i_th column of the homography induced by m_th plane  
function Q = NormLinearTran(M)
m = size(M,1);
Q = zeros(4,4);
%mean
c1 = sum(M(:,1))/m;
c2 = sum(M(:,2))/m;
c3 = sum(M(:,3))/m;
%centralize data
m1 = M(:,1) - c1;
m2 = M(:,2) - c2;
m3 = M(:,3) - c3;
%mean distance
d = 0;
for i = 1:m
d = d + sqrt(m1(i)^2 + m2(i)^2 + m3(i)^2);
end
d = d / m;
%re-scale factor
S = sqrt(3)/d;
Q(1:3,1:3) = eye(3)*S;
Q(4,:) = [-c1*S -c2*S -c3*S 1];
end

%low rank approximation
function Ara = RankApproximation(A,r)
[U,D,V] = svd(A);
[row,col] = size(D);
for i = r+1:col
    D(:,i) = zeros(row,1);
end
Ara = U*D*V';
end

%solve Ax = 0, here the rank of A can be n-2 or n-1 
function f = SolveLS(A)
    r = rank(A);
    [U,D,V] = svd(A);
    if r == 3
        f = V(:,end);
        return;
    end
    if r == 2
        a = V(end,end-1);
        b = V(end,end);
        c = [abs(a) abs(b)];
        [c id]= min(c);
        id = id - 1;
        if c < 10^(-5)
            f = V(:,end - 1 + id);
            return;
        end
        if id == 0
            x = -a/b;
            f = V(:,end-1) + x*V(:,end);
            return;
        end
        if id == 1
            x = -b/a;
            f = x*V(:,end-1)+V(:,end);
            return;
        end
    end 
    
    f = 1;
end

