function Tc_t_est = poseEstimation(P,p,cam)
%Function that estimates the pose of the object w.r.t. the camera
% from the points P on the object (w.r.t. object frame) and pixels p
% of the camera cam.
% Variable names are set to make it easy to understand following 
% Siciliano's book

%First compute the features si=[Xi,Yi]' from the pixels pi of the camera
% px=f*alphax*X+X0
% p(1,1)=(cam.f/cam.rho(1))*X1+cam.u0
% py=f*alphay*Y+Y0
% p(2,1)=(cam.f/cam.rho(2))*Y1+cam.v0
X1 = (p(1,1)-cam.u0)*(cam.rho(1)/cam.f);
Y1 = (p(2,1)-cam.v0)*(cam.rho(2)/cam.f);

X2 = (p(1,2)-cam.u0)*(cam.rho(1)/cam.f);
Y2 = (p(2,2)-cam.v0)*(cam.rho(2)/cam.f);

X3 = (p(1,3)-cam.u0)*(cam.rho(1)/cam.f);
Y3 = (p(2,3)-cam.v0)*(cam.rho(2)/cam.f);

X4 = (p(1,4)-cam.u0)*(cam.rho(1)/cam.f);
Y4 = (p(2,4)-cam.v0)*(cam.rho(2)/cam.f);

%Coordinates of the points on the object 
rx1 = P(1,1);
ry1 = P(2,1);
rx2 = P(1,2);
ry2 = P(2,2);
rx3 = P(1,3);
ry3 = P(2,3);
rx4 = P(1,4);
ry4 = P(2,4);

% Skew-symetric matrices Si
% Si=[0 -1 -Yi;1 0 -Xi; -Yi Xi 0]
S1=[0 -1 Y1; 1 0 -X1; -Y1 X1 0];
S2=[0 -1 Y2; 1 0 -X2; -Y2 X2 0];
S3=[0 -1 Y3; 1 0 -X3; -Y3 X3 0];
S4=[0 -1 Y4; 1 0 -X4; -Y4 X4 0];

% Ai=[rxi*Si ryi*Si Si]
A1=[rx1*S1 ry1*S1 S1];
A2=[rx2*S2 ry2*S2 S2];
A3=[rx3*S3 ry3*S3 S3];
A4=[rx4*S4 ry4*S4 S4];
A=[A1;A2;A3;A4];

% Solve using the singular value decomposition of A
[U,S,V] = svd(A);
H=[V(1:3,9) V(4:6,9) V(7:9,9)];

%psi with minus sign in order to locate the object in front of the camera,
%i.e. zp>0 
if H(3,3)>0
    sign=1;
else
    sign=-1;
end
psi=sign/norm(H(:,1)); 
H=H.*psi;
r1=H(:,1);
r2=H(:,2);
o=H(:,3);
r3=cross(r1,r2);
%disp('The esimated transform is:')
Tc_t_est=[r1 r2 r3 o; 0 0 0 1];
Tc_t_est=trnorm(Tc_t_est);