Image Based Visual Servoing algorithm in MATLAB - algorithm

Question

I was trying to implement the IBVS algorithm (the one explained in the Introduction here) in MATLAB myself, but I am facing the following problem : The algorithm seems to work only for the cases that the camera does not have to change its orientation in respect to the world frame.For example, if I just try to make one vertex of the initial (almost) square go closer to its opposite vertex, the algorithm does not work, as can be seen in the following image
The red x are the desired projections, the blue circles are the initial ones and the green ones are the ones I get from my algorithm.
Also the errors are not exponentially dereasing as they should.
What am I doing wrong? I am attaching my MATLAB code which is fully runable. If anyone could take a look, I would be really grateful. I took out the code that was performing the plotting. I hope it is more readable now. Visual servoing has to be performed with at least 4 target points, because else the problem has no unique solution. If you are willing to help, I would suggest you take a look at the calc_Rotation_matrix() function to check that the rotation matrix is properly calculated, then verify that the line ds = vc; in euler_ode is correct. The camera orientation is expressed in Euler angles according to this convention. Finally, one could check if the interaction matrix L is properly calculated.
function VisualServo()
global A3D B3D C3D D3D A B C D Ad Bd Cd Dd
%coordinates of the 4 points wrt camera frame
A3D = [-0.2633;0.27547;0.8956];
B3D = [0.2863;-0.2749;0.8937];
C3D = [-0.2637;-0.2746;0.8977];
D3D = [0.2866;0.2751;0.8916];
%initial projections (computed here only to show their relation with the desired ones)
A=A3D(1:2)/A3D(3);
B=B3D(1:2)/B3D(3);
C=C3D(1:2)/C3D(3);
D=D3D(1:2)/D3D(3);
%initial camera position and orientation
%orientation is expressed in Euler angles (X-Y-Z around the inertial frame
%of reference)
cam=[0;0;0;0;0;0];
%desired projections
Ad=A+[0.1;0];
Bd=B;
Cd=C+[0.1;0];
Dd=D;
t0 = 0;
tf = 50;
s0 = cam;
%time step
dt=0.01;
t = euler_ode(t0, tf, dt, s0);
end
function ts = euler_ode(t0,tf,dt,s0)
global A3D B3D C3D D3D Ad Bd Cd Dd
s = s0;
ts=[];
for t=t0:dt:tf
ts(end+1)=t;
cam = s;
% rotation matrix R_WCS_CCS
R = calc_Rotation_matrix(cam(4),cam(5),cam(6));
r = cam(1:3);
% 3D coordinates of the 4 points wrt the NEW camera frame
A3D_cam = R'*(A3D-r);
B3D_cam = R'*(B3D-r);
C3D_cam = R'*(C3D-r);
D3D_cam = R'*(D3D-r);
% NEW projections
A=A3D_cam(1:2)/A3D_cam(3);
B=B3D_cam(1:2)/B3D_cam(3);
C=C3D_cam(1:2)/C3D_cam(3);
D=D3D_cam(1:2)/D3D_cam(3);
% computing the L matrices
L1 = L_matrix(A(1),A(2),A3D_cam(3));
L2 = L_matrix(B(1),B(2),B3D_cam(3));
L3 = L_matrix(C(1),C(2),C3D_cam(3));
L4 = L_matrix(D(1),D(2),D3D_cam(3));
L = [L1;L2;L3;L4];
%updating the projection errors
e = [A-Ad;B-Bd;C-Cd;D-Dd];
%compute camera velocity
vc = -0.5*pinv(L)*e;
%change of the camera position and orientation
ds = vc;
%update camera position and orientation
s = s + ds*dt;
end
ts(end+1)=tf+dt;
end
function R = calc_Rotation_matrix(theta_x, theta_y, theta_z)
Rx = [1 0 0; 0 cos(theta_x) -sin(theta_x); 0 sin(theta_x) cos(theta_x)];
Ry = [cos(theta_y) 0 sin(theta_y); 0 1 0; -sin(theta_y) 0 cos(theta_y)];
Rz = [cos(theta_z) -sin(theta_z) 0; sin(theta_z) cos(theta_z) 0; 0 0 1];
R = Rx*Ry*Rz;
end
function L = L_matrix(x,y,z)
L = [-1/z,0,x/z,x*y,-(1+x^2),y;
0,-1/z,y/z,1+y^2,-x*y,-x];
end
Cases that work:
Ad=2*A;
Bd=2*B;
Cd=2*C;
Dd=2*D;
Ad=A+1;
Bd=B+1;
Cd=C+1;
Dd=D+1;
Ad=2*A+1;
Bd=2*B+1;
Cd=2*C+1;
Dd=2*D+1;
Cases that do NOT work:
Rotation by 90 degrees and zoom out (zoom out alone works, but I am doing it here for better visualization)
Ad=2*D;
Bd=2*C;
Cd=2*A;
Dd=2*B;

Answer 1

Your problem comes from the way you move the camera from the resulting visual servoing velocity. Rather than
cam = cam + vc*dt;
you should compute the new camera position using the exponential map
cam = cam*expm(vc*dt)