% initia the terminal
clear;
clc;
% the initial position
P = [10;10;10];
%% [u,v,1] = 1/s * K * (T * P)_1:3
% T = [R t;0 1]
% solve the u,v, set the T and K
%-------set the T-----
R = eul2rotm([0,-pi/2,0]);
t = [0;10;100];
%(T * P)_1:3 = R*P+t
%----set the K-----
fx = 1500;
fy = 1500;
cx = 500;
cy = 400;
K = [fx,0,cx;0,fy,cy;0,0,1];
sU = K * (R*P+t);
% the coordinate of image
U = sU/sU(3);
fprintf("the x of image is: %.4f\n",U(1));
fprintf("the y of image is: %.4f \n",U(2));