% Rc : current camera frame
% Rcd : desired camera frame
% Rcdc : rotation between desired and current camera frame

% Magnification of the camera

Gx=1000;
Gy=1000;

% Definition of the rotation : 0.1 rad around x

ux=1;
uy=0;
uz=0;

u=[ux uy uz];
th=0.1;
thu=th*u;

% Definition of the coss-product matrix

ucross=[0 -uz uy;uz 0 -ux;-uy ux 0];

% Definition of the Jacobian Lthu

Lthu=eye(3)-th/2*ucross+(1-(sinc(th/pi))/(sinc((th/2)/pi))^2)*ucross^2;

% Definition of Rcdc

vth=1-cos(th);
Rcdc=[ux^2*vth+cos(th) ux*uy*vth-uz*sin(th) ux*uz*vth+uy*sin(th);ux*uy*vth+uz*sin(th) uy^2*vth+cos(th) uy*uz*vth-ux*sin(th);ux*uz*vth-uy*sin(th) uy*uz*vth+ux*sin(th) uz^2*vth+cos(th)];

% Definition of tcdc

tcdc=[0.1 0 0.0];

% Definition of Mcdc

Mcdc=[ Rcdc tcdc';0 0 0 1];

% Desired camera-object transformation

Mcdo=[1 0 0 0.15;0 0 -1 0.1;0 1 0 0.3;0 0 0 1];

% Current camera-object transformation

Mco=inv(Mcdc)*Mcdo;

% Coordinates of the object points in the object frame

M0=[0 0 0]';
M1=[0.1 0 0]';
M2=[0 0.1 0]';
M3=[0 0 0.1]';

% Computation of the projections in the desired camera frame

m0d = Mcdo*[M0;1];
m1d = Mcdo*[M1;1];
m2d = Mcdo*[M2;1];
m3d = Mcdo*[M3;1];

x0d = Gx * m0d(1) / m0d(3);
y0d = Gy * m0d(2) / m0d(3);

x1d = Gx * m1d(1) / m1d(3);
y1d = Gy * m1d(2) / m1d(3);

x2d = Gx * m2d(1) / m2d(3);
y2d = Gy * m2d(2) / m2d(3);

x3d = Gx * m3d(1) / m3d(3);
y3d = Gy * m3d(2) / m3d(3);

Fd = [x0d y0d x1d y1d x2d y2d x3d y3d]';

Z0d=m0d(3);
Z1d=m1d(3);
Z2d=m2d(3);
Z3d=m3d(3);

% Computation of the projections in the current camera frame

m0 = Mco*[M0;1];
m1 = Mco*[M1;1];
m2 = Mco*[M2;1];
m3 = Mco*[M3;1];

x0 = Gx * m0(1) / m0(3);
y0 = Gy * m0(2) / m0(3);

x1 = Gx * m1(1) / m1(3);
y1 = Gy * m1(2) / m1(3);

x2 = Gx * m2(1) / m2(3);
y2 = Gy * m2(2) / m2(3);

x3 = Gx * m3(1) / m3(3);
y3 = Gy * m3(2) / m3(3);

F = [x0 y0 x1 y1 x2 y2 x3 y3]';

Z0=m0(3);
Z1=m1(3);
Z2=m2(3);
Z3=m3(3);

% Computation of the interaction matrix (image Jacobian)

LFd0=[-Gx/Z0d 0 x0d/Z0d x0d*y0d/Gy -(Gx^2+x0d^2)/Gx y0d*Gx/Gy;0 -Gy/Z0d y0d/Z0d (Gy^2+y0d^2)/Gy -x0d*y0d/Gx -x0d*Gy/Gx];
LFd1=[-Gx/Z1d 0 x1d/Z1d x1d*y1d/Gy -(Gx^2+x1d^2)/Gx y1d*Gx/Gy;0 -Gy/Z1d y1d/Z1d (Gy^2+y1d^2)/Gy -x1d*y1d/Gx -x1d*Gy/Gx];
LFd2=[-Gx/Z2d 0 x2d/Z2d x2d*y2d/Gy -(Gx^2+x2d^2)/Gx y2d*Gx/Gy;0 -Gy/Z2d y2d/Z2d (Gy^2+y2d^2)/Gy -x2d*y2d/Gx -x2d*Gy/Gx];
LFd3=[-Gx/Z3d 0 x3d/Z3d x3d*y3d/Gy -(Gx^2+x3d^2)/Gx y3d*Gx/Gy;0 -Gy/Z3d y3d/Z3d (Gy^2+y3d^2)/Gy -x3d*y3d/Gx -x3d*Gy/Gx];

LFd=[LFd0;LFd1;LFd2;LFd3];

% Computation of the pose difference : current (p) - desired (pd)

ppd=pinv(LFd)*(F-Fd);

% Print the pose variation

ppd(1:3)

ppd(4:6)/norm(ppd(4:6))

norm(ppd(4:6))