% Translation
T = [0;0;5];

% Rotation
R = [1,     0,          0;
     0,     cosd(20),   -sind(20);
     0,     sind(20),   cosd(20)];

% Calibration matrix
K = [800,   0,      250;
     0,     800,    250;
     0,     0,      1];

% Unit cube (the repeating points allow it to be plotted)
M = [0,0,0;
     1,0,0;
     1,1,0;
     0,1,0;
     0,0,0;
     0,0,1;
     1,0,1;
     1,1,1;
     0,1,1;
     0,0,1;
     1,0,1;
     1,0,0;
     1,1,0;
     1,1,1;
     0,1,1;
     0,1,0]';

% Calculate the projection
x = project(M,R,T,K);

% Plot the original cube and the projection
subplot(1,2,1)
plot3(M(1,:), M(2,:), M(3,:));
axis equal;
set(gcf, 'color', 'w');
xlabel('X_{world}')
ylabel('Y_{world}')
zlabel('Z_{world}')
title('Original cube')

subplot(1,2,2);
plot(x(1,:), x(2,:));
axis equal;
axis([0 500 0 500])
title('Projection')