# Image Based Visual Servoing algorithm in MATLAB

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
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

%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;


You are using a target set of image points that are impossible to achieve. Simply shifting two of the points in one direction within image coordinates does not correspond to a rigid transformation of your 3D set of points. It may make sense for orthographic projections in CAD software, but cameras have perspective so you need to include that in your transformation -- if the plate is rotating towards the camera the shifted points will also move outwards in the vertical direction. Here is a figure to clarify what I mean:

The easiest fix is to actually generate a true transformation in 3D (rotation and/or translation), then determine the new pixel coordinates and use that for testing -- this also gives you something against which you can check your solved camera transformation.

It also explains why zooming and translation work fine.

So, to be perfectly honest, I don't understand the application, but I'm very good with Matlab and controls.

That said, your code is really hard on the eyes. I'm not sure why you would jump straight to tracking 4 points before you got it working for 1, but that could be part of your application (again:ignorant).

It's hard to read mostly because of your naming of so many very similar variables and your inconsistent use of matrix operations. For instance, you define A through D as 3D(1:2)/3D(3) in your main code, but then you define x1,y1,x2...y4 in your euler function despite the fact that they are calculated the same way.

I would recommend you consider a structure array. Consider the following:

point(1).image = point(1).world(1:2)/point(1).world(3);


This does several things for you:

2. You can use as many or few points as you want - poll how many points you have with numel(points) and you can add more points with points(end+1)=....
3. You can operate on all of the points with one operation inside a for loop - points(i).var = ....
4. Most importantly - you can pass everything to do with your points in and out of your function as a function parameter rather than declaring them as global.

FINALLY - about your problem, the only thing I can see is at the last step - $v_c = -\lambda L_x e$. I don't see where this is calculated, but as this is a control problem I must assume that $\lambda$ refers to an eigenvalue, which are typically the poles of the system.

The poles dictate system response - a negative pole indicates a stable system, where a positive pole indicates an unstable system. Because your equation shows a negative $\lambda$, I would think that the entire term should be positive for a stable controller. Your term is negative, so that might be the cause of your problem, but again:ignorant.

SO, sorry for the lecture, but I think you might be finding that messy code is hard to debug. If you could find the derivation of the equation then I'd be happy to take a look at it, but I'm reading/posting from my phone and the TeX equations on that page are microscopic relative to the regular text so it's very hard for me to read. Try flipping the sign on your camera speed and if that doesn't fix try cleaning up your code and see if you've got some transposition error somewhere.

• The derivation of the control law is described shortly here irisa.fr/lagadic/visp/documentation/visp-2.10.0/…. I think that the problem lies either with the derivation of the rotation matrix or with the update of the camera position since when there is no rotation required everything is ok. – Controller Sep 5 '15 at 11:15
• I simplified the code in my initial post a little bit and I added information about which part probably causes the problem. Thank you for taking the time to reply. – Controller Sep 5 '15 at 11:32
• You can also find another article that describes the control law in its first 2 pages here www-cvr.ai.uiuc.edu/~seth/ResPages/pdfs/ChaHut06.pdf – Controller Sep 5 '15 at 12:46

There is a mistake in calculating ds; it should be: ds = L*vc instead of ds = vc

• Welcome to Robotics user16915. Thanks for your answer but we are looking for comprehensive answers that provide some explanation and context. One-line answers cannot do this, so please edit your answer to explain why it is right, ideally with citations. Answers that don't include explanations may be removed. – Mark Booth May 10 '17 at 11:07

This problem is called Chaumette Conundrum and it is a big negative of IBVS. You should try other VS algorithms.

• Welcome to Robotics.SE - could you please expand your answer, perhaps explaining what you mean? – Andrew Sep 9 '15 at 18:47
• We are looking for comprehensive answers that provide some explanation and context. Very short answers cannot do this, so please edit your answer to explain why it is right, ideally with citations. Answers that don't include explanations may be removed. – Mark Booth May 10 '17 at 11:07