%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%      Ortigosa Flores Jose Luis
%         Reealidad Aumentada
%               Tarea 5
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


disp("Puntos homogeinizados del modelo.");

P2_modelo = [-3, 3, -3,  3;
              3, 3, -3, -3;
              1, 1,  1,  1];
disp(P2_modelo);

P21_modelo = P2_modelo(:,1);
P22_modelo = P2_modelo(:,2);
P23_modelo = P2_modelo(:,3);
P24_modelo = P2_modelo(:,4);

disp("Puntos homogeinizados de la imagen.");


P1_imagen = [ 68, 180,  63, 183;
             119, 100, 257, 280; 
               1,   1,   1,   1];

disp(P1_imagen);

P11_imagen = P1_imagen(:,1);
P12_imagen = P1_imagen(:,2);
P13_imagen = P1_imagen(:,3);
P14_imagen = P1_imagen(:,4);

disp("Normalización de los vectores");
disp("Calculando T2");

P2x = P2_modelo(1,:);
P2mediaX = mean(P2x);
P2dsX = std(P2x, 1);

P2y = P2_modelo(2,:);
P2mediaY = mean(P2y);
P2dsY = std(P2y, 1);

T2 = [1/P2dsX,       0, -P2mediaX/P2dsX;
            0, 1/P2dsY, -P2mediaY/P2dsY;
            0,       0,              1];

disp(T2);

disp("Calculando T1");

P1x = P1_imagen(1,:);
P1mediaX = mean(P1x);
P1dsX = std(P1x, 1);

P1y = P1_imagen(2,:);
P1mediaY = mean(P1y);
P1dsY = std(P1y, 1);

T1 = [1/P1dsX,       0, -P1mediaX/P1dsX;
            0, 1/P1dsY, -P1mediaY/P1dsY;
            0,       0,              1];

disp(T1);

disp("Normalizo los vectores del modelo(P2), multiplicandolos por T2");

P21n_modelo = T2 * P21_modelo;
disp("P21");
disp(P21n_modelo);

P22n_modelo = T2 * P22_modelo;
disp("P22");
disp(P22n_modelo);

P23n_modelo = T2 * P23_modelo;
disp("P23");
disp(P23n_modelo);

P24n_modelo = T2 * P24_modelo;
disp("P24");
disp(P24n_modelo);

disp("Normalizo los vectores de la imagen(P1), multiplicandolos por T1");

P11n_imagen = T1 * P11_imagen;
disp("P11");
disp(P11n_imagen);

P12n_imagen = T1 * P12_imagen;
disp("P12");
disp(P12n_imagen);

P13n_imagen = T1 * P13_imagen;
disp("P13");
disp(P13n_imagen);

P14n_imagen = T1 * P14_imagen;
disp("P14");
disp(P14n_imagen);

disp("Calculo de la matriz A.");

%A=[x, y, 1, 0, 0, 0, -xu, -yu;
%   0, 0, 0, x, y, 1, -xv, -yv];

A=[P21n_modelo(1,1),P21n_modelo(2,1),1,0,0,0,-P21n_modelo(1,1)*P11n_imagen(1,1),-P21n_modelo(2,1)*P11n_imagen(1,1);
   0,0,0,P21n_modelo(1,1),P21n_modelo(2,1),1,-P21n_modelo(1,1)*P11n_imagen(2,1),-P21n_modelo(2,1)*P11n_imagen(2,1);
   P22n_modelo(1,1),P22n_modelo(2,1),1,0,0,0,-P22n_modelo(1,1)*P12n_imagen(1,1),-P22n_modelo(2,1)*P12n_imagen(1,1);
   0,0,0,P22n_modelo(1,1),P22n_modelo(2,1),1,-P22n_modelo(1,1)*P12n_imagen(2,1),-P22n_modelo(2,1)*P12n_imagen(2,1);
   P23n_modelo(1,1),P23n_modelo(2,1),1,0,0,0,-P23n_modelo(1,1)*P13n_imagen(1,1),-P23n_modelo(2,1)*P13n_imagen(1,1);
   0,0,0,P23n_modelo(1,1),P23n_modelo(2,1),1,-P23n_modelo(1,1)*P13n_imagen(2,1),-P23n_modelo(2,1)*P13n_imagen(2,1);
   P24n_modelo(1,1),P24n_modelo(2,1),1,0,0,0,-P24n_modelo(1,1)*P14n_imagen(1,1),-P24n_modelo(2,1)*P14n_imagen(1,1);
   0,0,0,P24n_modelo(1,1),P24n_modelo(2,1),1,-P24n_modelo(1,1)*P14n_imagen(2,1),-P24n_modelo(2,1)*P14n_imagen(2,1)];

disp("A=");
disp(A);
disp("V=");
V=[P11n_imagen(1,1);
   P11n_imagen(2,1);
   P12n_imagen(1,1);
   P12n_imagen(2,1);
   P13n_imagen(1,1);
   P13n_imagen(2,1);
   P14n_imagen(1,1);
   P14n_imagen(2,1)];

disp(V);

disp("Resolver Ah = V");
disp("h=(A^-1)V");

Ainv = inv(A);

h=Ainv * V;

disp("h=");

disp(h);

disp("Con el vector h formamos la matriz H.");
disp("Hprim=");

Hprim=[h(1,1),h(2,1),h(3,1);
       h(4,1),h(5,1),h(6,1);
       h(7,1),h(8,1),    1];

disp(Hprim);

disp("Para calcular H necesitamos calcular la inversa de T1 que es la siguiente:");

T1inv=inv(T1);

disp("T1^-1=");

disp(T1inv);

disp("Ahora calculamos H=(T1^-1)H'T2");

H=T1inv * Hprim * T2;

disp("La homografia es la siguiente:");

disp("H=");

disp(H);

disp("Verificación de la homografía");

disp("H*P21=P11");
disp(H * P21_modelo);
disp("El vector P11 real es:");
disp(P11_imagen);

disp("H*P22=P12");
disp(H * P22_modelo);
disp("El vector P12 real es:");
disp(P12_imagen);

disp("H*P23=P13");
disp(H * P23_modelo);
disp("El vector P13 real es:");
disp(P13_imagen);

disp("H*P24=P14");
disp(H * P24_modelo);
disp("El vector P14 real es:");
disp(P14_imagen);


