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:
A2=2*A; B2=2*B; C2=2*C; D2=2*D; A2=A+1; B2=B+1; C2=C+1; D2=D+1; A2=2*A+1; B2=2*B+1; C2=2*C+1; D2=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)
A2=2*D; B2=2*C; C2=2*A; D2=2*B;