Sunday, January 30, 2011



Today I would like to talk about a very interesting computer vision technique to control robots: Visual Servoing. Also called Vision-Based Robot Control, is a technique which uses the information gathered from a vision sensor (usually a camera) to control the motion of a robot.

A very good start point is the tutorial Visual Servo Control, Part I: Basic Approaches by F. Chaumette and S. Hutchinson published at IEEE Robotics and Automation Magazine, 13(4):82-90, December 2006.

The basic idea of a visual-based control scheme is to minimize the error between a set of measurements (usually a set of x,y coordinates of several image features) taken at the goal point of view and the set of measurements taken at the current point of view.

In this post I will not go into the details of the method, for that I refer you to the article cited above. But, instead I would like to show you a video, of the simulated robot I am working with, doing Visual Servo Control.

First the robot is activated and it learns how the target pattern "looks like" from the starting point of view (this will be the "goal point of view"). Then the target pattern is moved 6cm up. Obviously, at this position the point of view of the target that the camera sees is different than before, so the robot adapts itself to match the original point of view.

Later the target is moved 50cm towards the robot and again it moves itself so the goal point of view of the target is achieved. 

The demo has been developed using Microsoft Robotics Developer Studio and EmguCV

Sunday, January 9, 2011



One of the basic tasks in Computer Stereo Vision is to calibrate the stereo camera in order to obtain the parameters that will allow you to calculate 3D information of the scene.

Now, I could tell you a lot of stuff about camera projection models, stereoscopy, lens distortion, etc... but there is a lot of information available about such topics out there. So, this post is for those who simply need to calibrate a stereo camera system and start calculating 3D stuff right away by using OpenCV

Anyway, I strongly recommend you to read the book: Learning OpenCV: Computer Vision with the OpenCV Library by Gary Bradski and Adrian Kaehler, Published by O'Reilly Media, October 3, 2008.

So... what do I need to calibrate my stereo camera? A chessboard like this:

Why a chessboard? Because its corners are very easy to find by using computer vision algorithms and its geometry is very simple. In order to find out the position of any corner you only need to know how many horizontal and vertical squares there are in the chessboard and the size of a square. The chessboard in the image is a 9x6 chessboard and if you print it in a paper of size A4 the size of the squares would be more or less 2.5cm.

OK, I've printed my chessboard and I have measured the real size of the squares, now what?
Now you just take multiple views of the chessboard in different positions and orientations with your stereo camera using your favorite software (maybe your own software, software provided by your camera manufacturer or some other free software like Coriander). The images should look like this:

(Yeah, that is me in Hawaiian shorts on a summer day :P)
The more variety of positions and orientations of the checkerboard in the images the better.
Great, you have taken a lot of shots of the chessboard in different positions, now create a text file with the paths to the images. For example:

Now download this software and compile it.

It is just one of the examples of the book mentioned above that I modified to accept some configuration parameters and store the results of the calibration. The usage of the software is as follows:

USAGE: ./stereo_calibrate imageList nx ny squareSize
imageList : Filename of the image list (string). Example : list.txt
nx : Number of horizontal squares (int > 0). Example : 9
ny : Number of vertical squares (int > 0). Example : 6
squareSize : Size of a square (float > 0). Example : 2.5

So, in this example the call to the program stereo_calibrate would be:

./stereo_calibrate list.txt 9 6 2.5

The program will start showing the detected chessboards, calculate the calibration parameters and store them in a bunch of xml files:

D1.xml D2.xml
M1.xml M2.xml
mx1.xml mx2.xml
my1.xml my2.xml
P1.xml P2.xml
R1.xml R2.xml

Congratulations! You have calibrated your stereo camera!! Now you can load this parameters into any other program that uses that stereo camera and play with them:

CvMat *Q = (CvMat *)cvLoad("Q.xml",NULL,NULL,NULL);
CvMat *mx1 = (CvMat *)cvLoad("mx1.xml",NULL,NULL,NULL);
CvMat *my1 = (CvMat *)cvLoad("my1.xml",NULL,NULL,NULL);
CvMat *mx2 = (CvMat *)cvLoad("mx2.xml",NULL,NULL,NULL);
CvMat *my2 = (CvMat *)cvLoad("my2.xml",NULL,NULL,NULL);

Each of the files contains a matrix, if you would like to know the meaning of each matrix, please refer to the book at the beginning of this post. Right now, the useful stuff is contained on the files mx1.xml, my1.xml, mx2.xml, my2.xml and Q.xml.

The files m*.xml are the distortion models of the cameras. So we will need these matrices to undo the distortion of the images caused by the lens. Using the cvRemap() function:

cvRemap(imgLeftOrig, imgLeftUndistorted, mx1, my1);
cvRemap(imgRightOrig, imgRightUndistoreted, mx2, my2);

The goal of all this is to be able to calculate the 3D position (in meters, cm, mm or whatever magnitude you chose) of a point given its position (in pixels) on the left image and its correspondent on the right image. We are almost there, but for that we need the matrix Q. Given the position an interest point in the left and right image, its 3D position can be calculated as follows:

d = pointRightImage.X - pointLeftImage.X;

X = pointLeftImage.X * Q[0, 0] + Q[0, 3];
Y = pointLeftImage.Y * Q[1, 1] + Q[1, 3];
Z = Q[2, 3];
W = d * Q[3, 2] + Q[3, 3];

X = X / W;
Y = Y / W;
Z = Z / W;

And thats pretty much it, now you know how to calculate 3D positions from 2 images using OpenCV. How to find interest points in one image and its correspondent on the other is an art that will be explained another day ;)

EDIT [2011/06/16]: Many people asked me about a good book to get started in OpenCV, so take a look at this:  OpenCV 2 Computer Vision Application Programming Cookbook

EDIT [2011/08/22]: What next? Check out the following post: OpenCV: Stereo Matching

EDIT [2011/08/27]: I changed the Makefile of the software for one much simpler and without hard-coded stuff. Also hosted the software in my own server (it was in megaupload, sorry about that), the link to the software in this post has been updated. Here it is, just in case: 
EDIT [2012/01/05]: My hosting bandwidth was not enough to handle the traffic so I had to host the software on googlecode, the link to the software in this post has been updated. Here it is, just in case: