clc

%% Calcolo dell'omografia

x1 = [312,260,184,144,170,225,241,255,265,220,190,138; 
      289,311,246,196,176,226,226,190,152,113,133,94;
      1,1,1,1,1,1,1,1,1,1,1,1];
x2 = [335.9,256.8,129.5,39,116.8,179.8,256.8,256.2,256.8,200.5,142.9,59.14;
      263.5,297,268.2,195.18,154.3,201.9,233.4,192.5,138.2,62.5,68.5,16.9;
      1,1,1,1,1,1,1,1,1,1,1,1];



[F e1 e2] = fundmatrix(x1,x2);

% Scelgo i primi tre pti

y1 = x1(:,1);
y2 = x1(:,2);
y3 = x1(:,3);

y1_2 = x2(:,1);
y2_2 = x2(:,2);
y3_2 = x2(:,3);

% Calcolo A:

A_int = [0,-e2(3,1),e2(2,1);e2(3,1),0,-e2(1,1);-e2(2,1),e2(1,1),0];
A = A_int*F;
T = A*y1;
g = cross(y1_2,e2);
z = sqrt(g'*g);
b1 = ((cross(y1_2,T))'*(cross(y1_2,e2)))/z;



T2 = A*y2;
g2 = cross(y2_2,e2);
z2 = sqrt(g2'*g2);
b2 = ((cross(y2_2,T2))'*(cross(y2_2,e2)))/z2;

T3 = A*y3;
g3 = cross(y3_2,e2);
z3 = sqrt(g3'*g3);
b3 = ((cross(y3_2,T3))'*(cross(y3_2,e2)))/z3;


M = [y1';y2';y3'];

B =[b1,b2,b3];

v = inv(M)*B';

H = A - e2*v'

x2(:,1) - H*x1(:,1)

x1_om = vgg_get_homg(x1);
x2_om = vgg_get_homg(x2);

H12 = vgg_Haffine_from_x_MLE(x1_om,x2_om)

