funmat7p.m

计算基本矩阵的matlab例程

%funmat7p    Matriu Fonamental amb el metode dels 7 punts
%    [F] = funmat7p(M) 
%
%    M matriu de 4-per-7 amb les coordendes de 7 punts en dues imatges
%        primera fila: coordenada x de la primera imatge
%        segona fila:  coordenada y de la primera imatge
%        tercera fila: coordenada x de la segona imatge
%        quarta fila:  coordenada y de la segona imatge
%
%    F matriu fonamental de 3-per-3 amb l'origen de coordenades del m髇 a la segona camera
%
% by X. Armangue
% (c) Mr3D - University of Girona, September 2002
%

function [F]=funmat7p(M)

if (size(M,1)~=4) | (size(M,2)~=7),
   disp('Error: parametres incorrectes')
else
   U=[];
	for i=1:7,
	   U=[U; M(1,i)*M(3,i) M(1,i)*M(4,i) M(1,i) M(2,i)*M(3,i) M(2,i)*M(4,i) M(2,i) M(3,i) M(4,i) 1];
	end

	[UU,SS,VV]=svd(U);

	F1=[VV(1,8) VV(2,8) VV(3,8); VV(4,8) VV(5,8) VV(6,8); VV(7,8) VV(8,8) VV(9,8)];
	F2=[VV(1,9) VV(2,9) VV(3,9); VV(4,9) VV(5,9) VV(6,9); VV(7,9) VV(8,9) VV(9,9)];

	F111=F1(1,1);	F112=F1(1,2);	F113=F1(1,3);
	F121=F1(2,1);	F122=F1(2,2);	F123=F1(2,3);
	F131=F1(3,1);	F132=F1(3,2);	F133=F1(3,3);

	F211=F2(1,1);	F212=F2(1,2);	F213=F2(1,3);
	F221=F2(2,1);	F222=F2(2,2);	F223=F2(2,3);
	F231=F2(3,1);	F232=F2(3,2);	F233=F2(3,3);

	a=1;
	d=F221*F213*F232-F211*F223*F232+F211*F222*F233-F221*F212*F233+F231*F212*F223-F231*F213*F222;
	c=+F231*a*F112*F223+F231*F212*a*F123-3*F231*F212*F223*a-F231*a*F113*F222-F231*F213*a*F122+3*F231*F213*F222*a+a*F131*F212*F223-a*F131*F213*F222+a*F121*F213*F232-F221*a*F112*F233-F221*F212*a*F133+3*F221*F212*F233*a+F221*a*F113*F232+F221*F213*a*F132-3*F221*F213*F232*a+F211*a*F122*F233+F211*F222*a*F133-3*F211*F222*F233*a-F211*a*F123*F232-F211*F223*a*F132+3*F211*F223*F232*a-a*F121*F212*F233+a*F111*F222*F233-a*F111*F223*F232;
	b=+2*F231*F213*a^2*F122-3*F231*F213*a^2*F222+F231*a^2*F112*F123-2*F231*a^2*F112*F223-2*F231*F212*a^2*F123+3*F231*F212*a^2*F223-F231*a^2*F113*F122+2*F231*a^2*F113*F222+a^2*F131*F212*F123-2*a^2*F131*F212*F223-a^2*F131*F113*F222-a^2*F131*F213*F122+2*a^2*F131*F213*F222+a^2*F121*F113*F232+a^2*F121*F213*F132-2*a^2*F121*F213*F232-F221*a^2*F112*F133+2*F221*a^2*F112*F233+2*F221*F212*a^2*F133-3*F221*F212*a^2*F233+F221*a^2*F113*F132-2*F221*a^2*F113*F232-2*F221*F213*a^2*F132+3*F221*F213*a^2*F232+a^2*F131*F112*F223-2*F211*a^2*F122*F233-2*F211*F222*a^2*F133+3*F211*F222*a^2*F233-F211*a^2*F123*F132+2*F211*a^2*F123*F232+2*F211*F223*a^2*F132-3*F211*F223*a^2*F232-a^2*F121*F112*F233-a^2*F121*F212*F133+2*a^2*F121*F212*F233-2*a^2*F111*F222*F233-a^2*F111*F123*F232-a^2*F111*F223*F132+2*a^2*F111*F223*F232+a^2*F111*F122*F233+a^2*F111*F222*F133+F211*a^2*F122*F133;
	a=+F231*a^3*F112*F223+F231*a^3*F212*F123-F231*a^3*F212*F223+F231*a^3*F113*F122-F231*a^3*F113*F222-F231*a^3*F213*F122+F231*a^3*F213*F222-a^3*F131*F112*F223-a^3*F131*F212*F123+a^3*F131*F212*F223-a^3*F131*F113*F122+a^3*F131*F113*F222+a^3*F131*F213*F122-a^3*F131*F213*F222+a^3*F121*F212*F133-a^3*F121*F212*F233+a^3*F121*F113*F132-a^3*F121*F113*F232-a^3*F121*F213*F132+a^3*F121*F213*F232+F221*a^3*F112*F133-F221*a^3*F112*F233-F221*a^3*F212*F133+F221*a^3*F212*F233-F221*a^3*F113*F132+F221*a^3*F113*F232+F221*a^3*F213*F132-F221*a^3*F213*F232+a^3*F121*F112*F233-a^3*F111*F222*F133+a^3*F111*F222*F233-a^3*F111*F123*F132+a^3*F111*F123*F232+a^3*F111*F223*F132-a^3*F111*F223*F232-F211*a^3*F122*F133+F211*a^3*F122*F233+F211*a^3*F222*F133-F211*a^3*F222*F233+F211*a^3*F123*F132-F211*a^3*F123*F232-F211*a^3*F223*F132+F211*a^3*F223*F232+a^3*F111*F122*F133-a^3*F111*F122*F233-a^3*F121*F112*F133+a^3*F131*F112*F123-F231*a^3*F112*F123;

	sol=roots([a b c d]);
   
   if imag(sol(3))==0,
      F=sol(3)*F1+(1-sol(3))*F2;
   else
      F=sol(1)*F1+(1-sol(1))*F2;
   end   
end