function [H,Aeig,Beig]=hfrom2conics(Q1,Q2,S1,S2)
%
% hfrom2conics solves a planar homography from two conic
% correspondences: Q1 <-> S1, Q2 <-> S2 
%
% INPUT
%  Four real symmetric nonsingular 3x3 matrices:
%    Q1, Q2 - the conic coefficient matrices in the first view
%    S1, S2 - the corresponding conics in the second view
%
% OUTPUT
%   H - 3x3x4 matrix so that each H(:,:,i) is a solution (if a solution exists)
%   Aeig - the ordered eigenvalues of A
%   Beig - the ordered eigenvalues of B
%
% For details see the paper:
%
% Algorithms for computing a planar homography from conics in correspondence,
% J. Kannala, M. Salo and J. Heikkilä
% British Machine Vision Conference, 2006 (BMVC 2006)
%

Q1=Q1/norm(Q1,'fro');Q2=Q2/norm(Q2,'fro');
S1=S1/norm(S1,'fro');S2=S2/norm(S2,'fro');

kk1=det(S1)/det(Q1);
kk2=det(S2)/det(Q2);
ds1=(abs(kk1)^(1/3))*sign(kk1);
ds2=(abs(kk2)^(1/3))*sign(kk2);

Q1s=ds1*Q1;
Q2s=ds2*Q2;

[v,e]=eig(S2);
Fs2=v*sqrt(e);

[v,e]=eig(Q2s);
Fq2=v*sqrt(e);

A=Fq2.'*inv(Q1s)*Fq2;
B=Fs2.'*inv(S1)*Fs2;

[VA,DA]=eig(A);
da=diag(DA);
[VB,DB]=eig(B);
db=diag(DB);

dav=[real(da) imag(da)];
dbv=[real(db) imag(db)];
distmat=(dav(:,1)*ones(1,3)-ones(3,1)*dbv(:,1)').^2+(dav(:,2)*ones(1,3)-ones(3,1)*dbv(:,2)').^2;
orderings=[1 2 3; 1 3 2; 2 1 3; 2 3 1; 3 1 2; 3 2 1];
for l=1:6
  orderr(l)=sqrt(distmat(1,orderings(l,1))+distmat(2,orderings(l,2))+distmat(3,orderings(l,3)));
end
[minorderr,trueorder]=min(orderr);
dai=[1 2 3]'; dbi=(orderings(trueorder,:))';

VA=VA(:,dai'); DA=diag(da(dai));
VB=VB(:,dbi'); DB=diag(db(dbi));
Aeig=da(dai); Beig=db(dbi); 

tol=10^(-4);
if minorderr>tol % probably no solution
  solutionflag=1;  
end

LA=gramschmidt(VA);
LB=gramschmidt(VB);

P1=diag([1 1 1]); 
P2=diag([-1 1 1]);
P3=diag([1 -1 1]);
P4=diag([1 1 -1]);

R_1=LB*P1*LA.';
T1=(Fq2*R_1.'*inv(Fs2))';

R_2=LB*P2*LA.';
T2=(Fq2*R_2.'*inv(Fs2))';

R_3=LB*P3*LA.';
T3=(Fq2*R_3.'*inv(Fs2))';

R_4=LB*P4*LA.';
T4=(Fq2*R_4.'*inv(Fs2))';

H(:,:,1)=T1/det(T1);
H(:,:,2)=T2/det(T2);
H(:,:,3)=T3/det(T3);
H(:,:,4)=T4/det(T4);


function Z=gramschmidt(X)

Num=size(X,2);
n=size(X,1);
Z=X(:,1)/((X(:,1).'*X(:,1))^0.5);

for i=2:Num
  yi=X(:,i)-sum((ones(n,1)*(X(:,i).'*Z)).*Z,2);
  Z(:,i)=yi/((yi.'*yi)^0.5);
end
