% function [orth_img]=orth(I,f,phi,theta)
% %Orthoritification from oblique image
% % I: Input oblique image
% % f: Focal lens
% % phi: Shooting angle
% % theta: Field of view

clc;
clear all;
I = imread('IMG_0729.jpg');
f = 2.1177e+003;
phi = 80.955*2*pi/360;
theta = 0.2*2;

win = [5 5];

x = I(:,:,1);                %red
y = I(:,:,2);                %green
z = I(:,:,3);                %blue

R = medfilt2(x,win); 
G = medfilt2(y,win); 
B = medfilt2(z,win); 

I = cat(3,R,G,B); 
theta = theta/2;

s = size(I);
x0 = floor((s(2)+1)/2);   % the center of the image
y0 = floor((s(1)+1)/2);
ss(1) = abs(y0*(tan(phi+atan(y0*tan(theta)/y0))*cos(phi-theta)-sin(phi-theta))/sin(theta));
ss(2) = abs(x0*y0*cos(phi-theta)/(sin(theta)*cos(phi+theta)*((y0/tan(theta))^2+y0^2)^0.5) * 2);
ss(3) = s(3);

xx0 = floor(ss(2)/2);
ss(1) = floor(ss(1));
ss(2) = floor(ss(2));
II = zeros(ss);

scale_x = ss(2)/s(2);
t = 0;
for i=1:ss(1)
    for j=1:ss(2)
        x = j-xx0;
        y = ss(1)-i;
        
        yy = y0*(tan(atan((sin(theta)*y/y0+sin(phi-theta))/cos(phi-theta))-phi)+tan(theta))/tan(theta);
        xx = x *sin(theta)*(y0^2/tan((theta)^2)+(yy-y0)^2)^0.5/((y0*cos(phi-theta))^2+(y*sin(theta)+y0*sin(phi-theta))^2)^0.5;
        xx = uint32(xx+x0);
        yy = uint32(s(1)-yy);
        
%         if xx <= 1.0e-8
%             xx = 1;
%         end
%         if xx > s(2)
%             xx = s(2);
%         end
        if yy <= 1.0e-8
            yy = 1;
        end
        if yy > s(1)
            yy = s(1);
        end
        
        
        if xx >= 1 && xx <= s(2)
            
            t = t + 1;
            
            if xx/double(uint16(xx)) == 1 && yy/double(uint16(yy)) == 1
                RR(i,j) = double(R(yy,xx));
                GG(i,j) = double(G(yy,xx));
                BB(i,j) = double(B(yy,xx));
            else 
                xx_new = double(floor(xx));
                yy_new = double(floor(yy));
            
                R_1 = double(R(xx_new+1,yy_new))*(xx-xx_new)+double(R(xx_new,yy_new))*(xx_new+1-xx);
                R_2 = double(R(xx_new+1,yy_new+1))*(xx-xx_new)+double(R(xx_new,yy_new+1))*(xx_new+1-xx);
                RR(i,j) = uint8(R_1*(yy_new+1-yy)+R_2*(yy-yy_new));
            
                G_1 = double(G(xx_new+1,yy_new))*(xx-xx_new)+double(G(xx_new,yy_new))*(xx_new+1-xx);
                G_2 = double(G(xx_new+1,yy_new+1))*(xx-xx_new)+double(G(xx_new,yy_new+1))*(xx_new+1-xx);
                GG(i,j) = uint8(G_1*(yy_new+1-yy)+G_2*(yy-yy_new));        
            
                B_1 = double(B(xx_new+1,yy_new))*(xx-xx_new)+double(B(xx_new,yy_new))*(xx_new+1-xx);
                B_2 = double(B(xx_new+1,yy_new+1))*(xx-xx_new)+double(B(xx_new,yy_new+1))*(xx_new+1-xx);
                BB(i,j) = uint8(B_1*(yy_new+1-yy)+B_2*(yy-yy_new));
            end 
        end 
        
    end
end    

II(:,:,1) = RR;
II(:,:,2) = GG;
II(:,:,3) = BB;
II = uint8(II);
subplot(1,2,1),imshow(I);
subplot(1,2,2),imshow(II);



