
function [o1,o2]=LK(ORIGINAL_IMAGE_1,ORIGINAL_IMAGE_2,gaus_sigma);

[height,width]=size(ORIGINAL_IMAGE_1);
ORIGINAL_IMAGE_1=im2double(ORIGINAL_IMAGE_1);      
ORIGINAL_IMAGE_2=im2double(ORIGINAL_IMAGE_2);           
IMAGE_1_SMOOTHED=zeros(height,width);             
IMAGE_2_SMOOTHED=zeros(height,width);


Derx_1=zeros(height,width);             
Dery_1=zeros(height,width);             
Derx_2=zeros(height,width);             
Dery_2=zeros(height,width);             
Ix=zeros(height,width);              
Iy=zeros(height,width);     
It=zeros(height,width);


okoli=5;  %5                   
A=zeros(2,2);             
B=zeros(2,1);  
output1=zeros(height,width);         
output2=zeros(height,width);


velikost_jadra = 6*gaus_sigma+1;         
k = (velikost_jadra-1)/2;          
gaus_kernel_x=zeros(velikost_jadra,velikost_jadra);
gaus_kernel_y=zeros(velikost_jadra,velikost_jadra);
kernel=zeros(velikost_jadra,velikost_jadra);


for i=1:velikost_jadra

    for j=1:velikost_jadra

        gaus_kernel_x(i,j) = -( (j-k-1)/( 2* pi * gaus_sigma^3 ) ) * exp ( - ( (i-k-1)^2 + (j-k-1)^2 )/ (2*gaus_sigma^2) );

    end

end

 
for i=1:velikost_jadra

    for j=1:velikost_jadra

        gaus_kernel_y(i,j) = -( (i-k-1)/( 2* pi * gaus_sigma^3 ) ) *  exp ( - ( (i-k-1)^2 + (j-k-1)^2 )/ (2*gaus_sigma^2) );

    end

end

Derx_1 = filter2(gaus_kernel_x,ORIGINAL_IMAGE_1);
Dery_1 = filter2(gaus_kernel_y,ORIGINAL_IMAGE_1);
Derx_2 = filter2(gaus_kernel_x,ORIGINAL_IMAGE_2);
Dery_2 = filter2(gaus_kernel_y,ORIGINAL_IMAGE_2);

 

Ix = (Derx_1 + Derx_2) / 2;

Iy = (Dery_1 + Dery_2) / 2;

for i=1:velikost_jadra
    for j=1:velikost_jadra
        kernel(i,j) = (1/(2*pi*(gaus_sigma^2))) * exp (-((i-k-1)^2 + (j-k-1)^2)/(2*gaus_sigma^2));
    end
end

IMAGE_1_SMOOTHED = filter2(kernel,ORIGINAL_IMAGE_1);
IMAGE_2_SMOOTHED = filter2(kernel,ORIGINAL_IMAGE_2);
It = IMAGE_2_SMOOTHED - IMAGE_1_SMOOTHED;


for i=(1+floor(okoli/2)):(height-floor(okoli/2))
    for j=(1+floor(okoli/2)):(width-floor(okoli/2))
        A=zeros(2,2);
        B=zeros(2,1);
        for m=i-floor(okoli/2):i+floor(okoli/2)

            for n=j-floor(okoli/2):j+floor(okoli/2)

               

                A(1,1)=A(1,1) + Ix(m,n)*Ix(m,n);

                A(1,2)=A(1,2) + Ix(m,n)*Iy(m,n);
                A(2,1)=A(2,1) + Ix(m,n)*Iy(m,n);
                A(2,2)=A(2,2) + Iy(m,n)*Iy(m,n);
                B(1,1)=B(1,1) + Ix(m,n)*It(m,n);
                B(2,1)=B(2,1) + Iy(m,n)*It(m,n);

               

            end

        end

        Ainv=pinv(A);

        result=Ainv*(-B);

        output1(i,j)=result(1,1);

        output2(i,j)=result(2,1);

       

    end

end


 

output1=flipud(output1);
output2=flipud(output2); 

o1=output1;
o2=output2;
