function [SOLS] = sm23new( Q ) 
%function [SOLS] = sm23new( Q ) 
%
% Q is a matrix containing observations on plucker form from 
%  2 stations with 3 points visible per station. It should be 8x6
%
% The columns of V are the solutions. Given a motion T
%   the first two elements is the rotation [a b;-b a] and the last 
%   two are the translation [c;d]; 
%
%
%
%
%
%
%Copyright Henrik Stewenius, 2005. All Rights Reserved.
%
%Permission to use [*] this software for academic purposes is hereby
%granted without fee or royalty, subject to the following conditions.
%
%1. The software is provided "as is" by the copyright holder, with
%absolutely no actual or implied warranty of correctness, fitness,
%intellectual property ownership, or anything else whatsoever. You use
%the software entirely at your own risk. In no event shall the copyright
%holder be liable for any direct, indirect or perceived damage whatsoever
%connected with the use of this work.
%
%2. The software may not be distributed [*], whether in its original
%or modified form, without explicit written consent from the copyright
%holder.
%
%3. The copyright holder retains all copyright to the work.
%
%4. Any scholarly work, research or publication making use of this
%software must explicitly acknowledge the copyright holder. In the
%case of a publication, citation of the work, and author thereof,
%titled:
%@PHDTHESIS{stewenius-pdh-2005,
%     AUTHOR = {H. Stew\'enius},
%     TITLE = {Gr{\"o}bner Basis Methods for Minimal Problems in Computer Vision},
%     SCHOOL = {Lund University},
%     YEAR = 2005,
%     MONTH = APR,
%     PDF = {http://www.maths.lth.se/matematiklth/personal/stewe/THESIS/stewenius_phd_2005.pdf}
%}
%is deemed sufficient acknowledgement.
%
%Permission to use or distribute [*] this software for commercial or
%military purposes, or under any conditions not covered by the conditions
%1-4 above, is NOT granted. Such permission may be obtained subject to
%explicit prior written consent from the copyright holder.
%
%[*] `Use' denotes any use whatsoever including downloading, compilation,
%execution and modification. `Distribution' includes any distribution of
%the software and any derived works such as object files, executable
%programs and libraries.




Q1 = Q(3:end,1:3);
Q2 = Q(3:end,4:6);

%The monial order used in this solver ([a,b,c,d])
%MONS = [  1 1 0 0 1 0 0 0 0  
%	   0 0 1 1 0 1 0 0 0 
%	   1 0 1 0 0 0 1 0 0 
%	   0 1 0 1 0 0 0 1 0 ];

%%Posing the start set of equations
for i=1:3
   q1  =  Q1(1:3,i);
   q1p =  Q1(4:6,i);
   q2  =  Q2(1:3,i);
   q2p =  Q2(4:6,i);
    K(i,:) =[ -q1(3)*q2(2),q1(3)*q2(1),-q1(3)*q2(1),-q1(3)*q2(2),q1p(1)*q2(1)+q1p(2)*q2(2)+q1(1)*q2p(1)+q1(2)*q2p(2),-q1p(1)*q2(2)+q1p(2)*q2(1)-q1(1)*q2p(2)+q1(2)*q2p(1),q2(3)*q1(2),-q2(3)*q1(1),q2(3)*q1p(3)+q2p(3)*q1(3)];
end

%%Gaussian elimination
K(1,:) = K(1,:) /K(1,1);
for i=2:3
   K(i,:) = K(i,:)-K(i,1)*K(1,:);
   K(i,:) = K(i,:)/K(i,2);
end
K(1,:) = K(1,:) - K(1,2)*K(2,:);
K(3,:) = K(3,:) -K(2,:);
K(3,:) = K(3,:) / K(3,8);
for i=1:2
   K(i,:) = K(i,:)-K(i,8)*K(3,:);
end

%%We now use substitution to remove "d". 
%% 

%% We also change to a new monomial order ([a,b,c])
%MONS2 = [ 2 1 1 0 0 1 0 0 0 
%	  0 1 0 2 1 0 1 0 0 
%	  0 0 1 0 1 0 0 1 0 ];
%

asub = -K(3,5);
bsub = -K(3,6);
csub = -K(3,7);
sub1 = -K(3,9);

for i=1:2
   k = K(i,:);
   L(i,:) = [ k(2)*asub , ...
	      k(2)*bsub+k(4)*asub, ...
	      k(1)+k(2)*csub, ...
	      k(4)*bsub, ...
	      k(3) + k(4)*csub,...
	      k(5)+k(8)*asub+k(2)*sub1,...
	      k(6)+k(8)*bsub+k(4)*sub1,...
	      k(7)+k(8)*bsub,...
	      k(9)+k(8)*sub1 ];
end

   %%Adding the constraint  a^2+b^2-1 =0
L=[L; 
   1 0 0 1 0 0 0 0 -1 ];

%%More Gaussian elimination
L(2,:) = L(2,:) - L(2,1)*L(3,:);
L(2,:) = L(2,:) /L(2,2);
L(1,:) = L(1,:) - L(1,2)*L(2,:);
L(1,:) = L(1,:) /L(1,3);
L(2,:) = L(2,:) - L(2,3)*L(1,:);



%%Now time to multiply these equations by a,b and c. 
%%Probably no nead to multiply equation (row) 3 with 
%%   a since this is the only possible source for a^3

%%Creating multiplication matrices for multiplication of a 
%% polynomial by one of the monomials. 
%% (remember that in K[x] multiplication by a polynomial
%% is a linear operator. 
%%The matrices were built using MAPLE. 
Ma = zeros(19,9);
Mb = zeros(19,9);
Mc = zeros(19,9);
M1 = zeros(19,9);
M1([10 30 50 70 90 111 131 151 171]) = 1;
Ma([1 21 41 61 81 105 125 145 168])=1;
Mb([2 23 43 64 84 106 127 147 169])=1;
Mc([3 24 44 65 85 107 128 148 170])=1;

%%Actually performing the multiplication
Lnew= [(Ma*L(1:2,:)')'
       (Mb*L')'
       (Mc*L(3,:)')'
       (M1*L')' ];

%I = find( sum(abs( Lnew)));
I = [2 3 4 5 7 8 10 11 12 13 14 16 17 18 19];

%% Selecting the interesting colmumns. That is, those that are non-zero. 
Lnew = Lnew(:,I);
%MONS3= MONS3(:,I);

%% Here it is possible to throw column 5 and 6 since they do 
%% not interfere with later computations. 
B = Lnew(: , [1:4 7:11]);
A = inv(B)*Lnew;

%%We now have a Grobner basis in A 
%%Time to extract the matrix for multiplication by "a" 
%% modulo the Ideal formed by our equations. 
%%We know where it is and proceed immediately to the eigenproblem. 
[V,D]  = eig( [-A( 5:7, end-3:end)  ; 1 0 0 0 ]  );

V = V./(ones(4,1)*V(end,:));

%The variables a,b,c are found among the eigenvectors
avec = V(1,:);
bvec = V(2,:);
cvec = V(3,:);

%Since d was eliminated earlier we have to recover it. 
dvec = asub*avec + bsub*bvec+csub*cvec+sub1;

%Assembeling the solutions
SOLS = [avec;bvec;cvec;dvec];