% 2.3 World to screen transformation (3D/2D)

clear;
close all;

% Rotation angles (radians)
a = 10*pi/180;
b = 20*pi/180;
c = 30*pi/180;
% Translation parameters (mm)
tx = 50;
ty = 60;
tz = 500;

% Input: a point in Body frame coordinates
pb = [-20;30;50];

% Homogeneous coordinates of pb
pbH = [pb;1];

% Call the Euler Angles function (exercise 2.1)
R = Euler2Mat(a,b,c);

% Put the roto-translation into the
% homogeneous transformation matrix (4x4)
T = [R, [tx;ty;tz]; 0,0,0,1];

% Extrinsic transformation
pcH = T*pbH;

% Result: point in camera frame coordinates
pc = pcH(1:3);

% 2.4 Intrinsic transformation

% Intrinsic parameters

% Focal length
f = 1000;
% Horizontal and vertical resolutions
rx = 640;
ry = 480;

% Put them together into the intrinsic transformation matrix
K = [f 0 rx/2; 0 f ry/2; 0 0 1];

% Do the 3D/2D projection, from camera to screen
qH = K*pc;

% Normalize the homogeneous coordinates
q = [qH(1)/qH(3); qH(2)/qH(3)]

% Result: screen point q

% 2.5 Global transformation

% The two transformations can be combined
% into the Projection matrix (4x3)
P = K*[R, [tx;ty;tz]];

% The same process is done in a single step
% (in homogeneous coordinates)
qH = P*pbH;

q = [qH(1)/qH(3); qH(2)/qH(3)]
