% 
% %designed for the good of the world by Philip Torr based on ideas contained in 
% copyright Philip Torr and Microsoft Corp 2002
% 


% /*
% *****************************************
% */This gives a new method for estimating F that I developed (see citations below) from 7 points:

% 
% thus we can get a one parameter family of solutions (modulo scale) F = F1 + a F2
% exploiting  det | F | = 0 
% this gives us 1,2 or 3 distinct solutions for a

%MORE DETAILS IN:
% @phdthesis{Torr:thesis,
%         author="Torr, P. H. S.",
%         title="Outlier Detection and Motion Segmentation",
%         school=" Dept. of  Engineering Science, University of Oxford",
%         year=1995}
% 
% 
% @article{Torr97c,
%         author="Torr, P. H. S.  and Murray, D. W. ",
%         title="The Development and Comparison of Robust Methods for Estimating the Fundamental Matrix",
%         journal="IJCV",
%         volume = 24,
%         number = 3,
%         pages = {271--300},
%         year=1997
% }

%A is a 7 x 9 design matrix;
%big_result is a 3x9 matrix of putative F's

% returns 0,1,2 or 3 the number of fits gained.


% 
function [no_F, big_result] = torr_F_constrained_fit(x1,y1,x2,y2,m3)

A(:,1) = x1(:).* x2(:);
A(:,2) = y1(:).* x2(:);
A(:,3) = m3* x2(:);

A(:,4) = x1(:).* y2(:);
A(:,5) = y1(:).* y2(:);
A(:,6) = m3* y2(:);

A(:,7) = x1(:).* m3;
A(:,8) = y1(:).* m3;
A(:,9) = m3* m3;

no_F = 0;
sizeA = size(A);
if ((sizeA(1) ~= 7) | (sizeA(2) ~= 9))
    error('A is wrong shape');
    return;
end


[U,S,V] = svd(A);

%the two solutions 
r1 = V(:,length(V));
r2 = V(:,length(V)-1);

%  we now want to chose a solution so that the constrtaint
%     | aF_1 + (1-a)F_2 | = 0 is satisfies --- this gives us a cubic in a 

%coefficients of c_1 a^3 + c_2 a^2 + c_3 a + c_4
cubic = zeros(4,1);

cubic = torr_accumulate_coeff(cubic, r1, r2,1,5,9, 1);
cubic = torr_accumulate_coeff(cubic, r1, r2,1,6,8, 0);
cubic = torr_accumulate_coeff(cubic, r1, r2,2,4,9, 0);
cubic = torr_accumulate_coeff(cubic, r1, r2,2,6,7, 1);
cubic = torr_accumulate_coeff(cubic, r1, r2,3,4,8, 1);
cubic = torr_accumulate_coeff(cubic, r1, r2,3,5,7, 0);

%    /* cubic(0) x^3 + cubic(1) x^2 + cubic(2) x + cubic(3) etc */
a_roots = roots(cubic);

for i_root = 1:3
    if isreal(a_roots(i_root))
        no_F = no_F + 1;
        big_result(no_F,:) = r1(:)' * a_roots(i_root) + r2(:)' * (1 - a_roots(i_root));
    end
end





%accumulates the coefficients for the 
function  cubic_out = torr_accumulate_coeff(cubic,f1, f2, a, b, c, plus)
temp(1) = f1(a) * f1(b) * f1(c);
temp(2) = f1(a) * f1(b) * f2(c);
temp(3) = f1(a) * f2(b) * f1(c);
temp(4) = f1(a) * f2(b) * f2(c);
temp(5) = f2(a) * f1(b) * f1(c);
temp(6) = f2(a) * f1(b) * f2(c);
temp(7) = f2(a) * f2(b) * f1(c);
temp(8) = f2(a) * f2(b) * f2(c);

if plus
    cubic_out(1) = cubic(1) + temp(1) - temp(2) - temp(3) + temp(4) - temp(5) + temp(6) + temp(7) - temp(8);
    cubic_out(2) = cubic(2) + temp(2) + temp(3) - 2*temp(4) + temp(5) - 2*temp(6) - 2*temp(7)+ 3*temp(8); 
    cubic_out(3) = cubic(3) +temp(4) + temp(6) + temp(7) - 3 * temp(8);
    cubic_out(4) = cubic(4) +temp(8);
else
    cubic_out(1) = cubic(1) -temp(1) - temp(2) - temp(3) + temp(4) - temp(5) + temp(6) + temp(7) - temp(8);
    cubic_out(2) = cubic(2) -temp(2) + temp(3) - 2*temp(4) + temp(5) - 2*temp(6) - 2*temp(7)+ 3*temp(8); 
    cubic_out(3) = cubic(3) -temp(4) + temp(6) + temp(7) - 3 * temp(8);
    cubic_out(4) = cubic(4) -temp(8);
end