% Definition of the focal length

f=0.01

% Coordinates of the object points into Ro

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

% DEfinition of A and its inverse

A=[M1'; M2'; M3']
B=inv(A)

% Definition of the camera->object transformation

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

% Computation of the projections in the image

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

x0=f*m0(1)/m0(3)
y0=f*m0(2)/m0(3)

x1=f*m1(1)/m1(3)
y1=f*m1(2)/m1(3)

x2=f*m2(1)/m2(3)
y2=f*m2(2)/m2(3)

x3=f*m3(1)/m3(3)
y3=f*m3(2)/m3(3)

% Init.

e1=0;e2=0;e3=0

% First iteration

disp('First iteration :' )

xp=[x1*(1+e1)-x0;x2*(1+e2)-x0;x3*(1+e3)-x0]
yp=[y1*(1+e1)-y0;y2*(1+e2)-y0;y3*(1+e3)-y0]

I=B*xp
J=B*yp

i=I/norm(I)
j=J/norm(J)

k=cross(i,j)

Z0=2*f/(norm(I)+norm(J))

e1=1/Z0*sum(M1.*k)
e2=1/Z0*sum(M2.*k)
e3=1/Z0*sum(M3.*k)

% Second iteration

disp('Second iteration :' )

xp=[x1*(1+e1)-x0;x2*(1+e2)-x0;x3*(1+e3)-x0]
yp=[y1*(1+e1)-y0;y2*(1+e2)-y0;y3*(1+e3)-y0]

I=B*xp
J=B*yp

i=I/norm(I)
j=J/norm(J)

k=cross(i,j)

Z0=2*f/(norm(I)+norm(J))

e1=1/Z0*sum(M1.*k)
e2=1/Z0*sum(M2.*k)
e3=1/Z0*sum(M3.*k)

% Third iteration

disp('Third iteration :' )

xp=[x1*(1+e1)-x0;x2*(1+e2)-x0;x3*(1+e3)-x0]
yp=[y1*(1+e1)-y0;y2*(1+e2)-y0;y3*(1+e3)-y0]

I=B*xp
J=B*yp

i=I/norm(I)
j=J/norm(J)

k=cross(i,j)

Z0=2*f/(norm(I)+norm(J))

e1=1/Z0*sum(M1.*k)
e2=1/Z0*sum(M2.*k)
e3=1/Z0*sum(M3.*k)

% Fourth iteration

disp('Fourth iteration :' )

xp=[x1*(1+e1)-x0;x2*(1+e2)-x0;x3*(1+e3)-x0]
yp=[y1*(1+e1)-y0;y2*(1+e2)-y0;y3*(1+e3)-y0]

I=B*xp
J=B*yp

i=I/norm(I)
j=J/norm(J)

k=cross(i,j)

Z0=2*f/(norm(I)+norm(J))

e1=1/Z0*sum(M1.*k)
e2=1/Z0*sum(M2.*k)
e3=1/Z0*sum(M3.*k)

% Fifth interation

disp('Fifth interation :' )

xp=[x1*(1+e1)-x0;x2*(1+e2)-x0;x3*(1+e3)-x0]
yp=[y1*(1+e1)-y0;y2*(1+e2)-y0;y3*(1+e3)-y0]

I=B*xp
J=B*yp

i=I/norm(I)
j=J/norm(J)

k=cross(i,j)

Z0=2*f/(norm(I)+norm(J))

e1=1/Z0*sum(M1.*k)
e2=1/Z0*sum(M2.*k)
e3=1/Z0*sum(M3.*k)