%Programa:      CalcularDespGlobal_Taylor.m
%Descripción:   Calcular el desplazamiento global entre dos imágenes usando
%               la aproximación de Taylor de primer orden
%Autor:         FJHL
%Act:           06/Sep/2018

clear all;close all;clc;

I1=im2double(rgb2gray(imread('../images/rect_1_sub.png')));
I2=im2double(rgb2gray(imread('../images/rect_2_sub.png')));

%Derivada con respecto del tiempo
dI2_dt=I2-I1;
%Derivadas espaciales (Dx, Dy)
[rows,cols]=size(I1);
dI1_dx=zeros(rows,cols);
dI1_dy=zeros(rows,cols);
for x=2:cols
    for y=2:rows
        dI1_dx(y,x)=I1(y,x)-I1(y,x-1);
        dI1_dy(y,x)=I1(y,x)-I1(y-1,x);
    end
end

%Calcular matriz A:
a11= sum(sum(dI1_dx.^2));
a12= sum(sum(dI1_dx.*dI1_dy));
a21= a12;%Ya que la matriz es simetrica
a22= sum(sum(dI1_dy.^2));
A=[a11 a12;a21 a22];

%Calcular el vector de terminos independientes
b1= sum(sum(dI1_dx.*dI2_dt));
b2= sum(sum(dI1_dy.*dI2_dt));
b=[b1 b2]';

%Calcular d=(dx,dy)
%Por Cramer:
detA=A(1,1)*A(2,2)-A(1,2)*A(2,1);
dx=(b1*a22-a12*b2)/detA;
dy=(a11*b2-a21*b1)/detA;

%d=inv(A)*b;
d=A\b;

%Aplicar el desplazamiento encontrado a la imagen I1.
I1_new=I1;
for x=3:cols-3
    for y=3:rows-3
        x_new=x+dx;
        y_new=y+dy;
        %I_new(y,x)=I1(round(y_new),round(x_new));
        I1_new(y,x)=interp2(I1,x_new,y_new);
    end
end

%Imagenes Residuales
Ir1=I2-I1;
Ir2=I2-I1_new;


figure(1);
imagesc(I1);
colormap gray;
figure(2);
imagesc(I2);
colormap gray;
figure(3);
imagesc(abs(dI1_dx));
colormap gray;
figure(4);
imagesc(abs(dI1_dy));
colormap gray;
figure(5);
imagesc(I1_new);
colormap gray;
figure(6);
imagesc(Ir1);
colormap gray;
figure(7);
imagesc(Ir2);
colormap gray;

