I treat this as a kind of tutorial, or a toy example, of how to perform Structure from Motion in OpenCV.

Let’s get down to business…

Getting a motion map

The basic thing when doing reconstruction from pairs of images, is that you know the motion: How much “a pixel has moved” from one image to the other. This gives you the ability to reconstruct it’s distance from the camera(s). So our first goal is to try and understand that from a pair of two images.

In calibrated horizontal stereo rigs this is called Disparity, and it refers to the horizontal motion of a pixel. And OpenCV actually has some very good tools to recover horizontal disparity, that can be seen in this sample.

But in our case we don’t have a calibrated rig as we are doing monocular (one camera) depth reconstruction, or in other words: Structure from motion.

You can go about getting a motion map in many different ways, but two canonical ways are: optical flow and feature matching.
Also, I will stick to what OpenCV has to offer, but obviously there is a whole lot of work.

Input pair of images, rotation and translation is unknown

Optical Flow

In optical flow you basically try to “track the pixels” from image 1 to 2, usually assuming a pixel can move only within a certain window in which you will search. OpenCV offers some ways to do optical flow, but I will focus on the newer and nicer one: Farenback’s method for dense optical flow.
The word dense means we look for the motion for every pixel in the image. This is usually costly, but Farneback’s method is linear which is easy to solve, and they have a rocking implementation of it in OpenCV so it basically flies.
Running the function on two images will provide a motion map, however my experiments show that this map is wrong in a fair bit of the times… To cope with that, I am doing an iterative operation, also leveraging the fact the this OF method can use an initial guess.
An example of using Farneback method exists in the samples directory of OpenCV’s repo: here.

Dense O-F using Farneback

Feature Matching

The other way of getting motion is matching features between the two images.
In each image we extract salient features and invariant descriptors, and then match the two sets of features.
It’s very easily done in OpenCV and widely covered by examples and tutorials.
This method however, will not provide a dense motion map. It will provide a very sparse one at best… so that depth reconstruction will also be sparse. We may talk about how to overcome that by hacking some segmentation methods, like superpixels and graph-cuts, in a different post.

SURF features matching, with Fundamental matrix pruning via RANSAC

A hybrid method

Another way that I am working on to get motion is a hybrid between Feature Matching and Optical Flow.
Basically the idea is to perform feature matching at first, and then O-F. When the motion is big, and features move quite a lot in the image, O-F sometimes fails (because pixel movement is usually confined to a search window).
After we get features pairs, we can try to recover a global movement in the image. We use that movement as an initial guess for O-F.

The rigid transform flow recovered from sparse feature matching

Estimating Motion

Once we have a motion map between the two images, it should pose no problem to recover the motion of the camera. The motion is described in the 3×4 matrix P, which is combined of two elements: P = [R|t], which are the Rotational element R and Translational element t.
H&Z give us a bunch of ways of recovering the P matrices for both cameras in Chapter 9 of their book. The central method being – using the Fundamental Matrix. This special 3×3 matrix encodes the epipolar constraint between the images, to put simply: for each point x in image 1 and corresponding point x’ in image 2 the following equation holds: x’Fx = 0.
How does that help us? Well H&Z also prove that if you have F, you can infer the two P matrices. And, if you have (sufficient) point matches between images, which we have, you can find F! Hurray!
This is simply visible in the linear sense. F has 9 entries (but only 8 degrees of freedom), so if we have enough point pairs, we can solve for F in a least squares sense. But… F is better estimated in a more robust way, and OpenCV takes care of all of this for us in the function findFundamentalMat. There are several methods for recovering F there, linear and non-linear.
However, H&Z also point to a problem with using F right away – projective ambiguity. This means that the recovered camera matrices may not be the “real” ones, but instead have gone through some 3D projective transformation. To cope with this, we will use the Essential Matrix instead, which is sort of the same thing (holds epiploar constraint over points) but for calibrated cameras. Using the Essential matrix removes the projective ambiguity and provides a Metric (or Singular) Reconstruction, which means the 3D points are true up to scaling alone, and not up to a projective transformation.

cv::FileStorage fs;
fs.open("camera_calibration.yml",cv::FileStorage::READ);
fs["camera_matrix"]>>K;

Mat F = findFundamentalMat(imgpts1, imgpts2, FM_RANSAC, 0.1, 0.99, status);
Mat E = K.t() * F * K; //according to HZ (9.12)

Now let’s assume one camera is P = [I|0], meaning it hasn’t moved or rotated, getting the second camera matrix, P’ = [R|t], is done as follows:

SVD svd(E);
Matx33d W(0,-1,0,	//HZ 9.13
	  1,0,0,
	  0,0,1);
Matx33d Winv(0,1,0,
	 -1,0,0,
	 0,0,1);
Mat_ R = svd.u * Mat(W) * svd.vt; //HZ 9.19
Mat_ t = svd.u.col(2); //u3
P1 = Matx34d(R(0,0),	R(0,1),	R(0,2),	t(0),
		 R(1,0),	R(1,1),	R(1,2),	t(1),
		 R(2,0),	R(2,1),	R(2,2), t(2));

Looks good, now let’s move on to reconstruction.

Reconstruction via Triangulation

Once we have two camera matrices, P and P’, we can recover the 3D structure of the scene. This can be seen simply if we think about it using ray intersection. We have two points in space of the camera centers (one in 0,0,0 and one in t), and we have the location in space of a point both on the image plane of image 1 and on the image plane of image 2. If we simply shoot a ray from from one camera center through the respective point and another ray from the other camera – the intersection of the two rays must be the real location of the object in space.
In real life, none of that works. The rays usually will not intersect (so H&Z refer to the mid-point algorithm, which they dismiss as a bad choice), and ray intersection in general is inferior to other triangulation methods.
H&Z go on to describe their “optimal” triangulation method, which optimizes the solution based on the error from reprojection of the points back to the image plane.
I have implemented the linear triangulation methods they present, and wrote a post about it not long ago: Here.
I also added the Iterative Least Squares method that Hartley presented in his article “Triangulation“, which is said to perform very good and very fast.

"Depth Map"

3D reconstruction

A word of notice, many many times the reconstruction will fail because the Fundamental matrix came out wrong. The results will just look aweful, and nothing like a true reconstruction. To cope with this, you may want to insert a check that will make sure the two P matrices are not completely bogus (you could check for a reasonable rotation for example). If the P matrices, that are derived from the F matrix, are strange, then you can discard this F matrix and compute a new one.

Example of when things go bad...

Toolbox and Framework

I created a small toolbox of the various methods I spoke about in this post, and created a very simple UI. It basically allows you to load two images and then try the different methods on them and get the results.
It’s using FLTK3 for the GUI, and PCL (VTK backend) for visualization of the result 3D point cloud.
It also includes a few classes with a simple API that let’s you get the features matches, motion map, camera matrices from the motion, and finally the 3D point cloud.

FLTK GUI

Code & Where to go next

The code, as usual, is up for grabs at github:

https://github.com/royshil/SfM-Toy-Library

Now, that have a firm grasp of SfM you can go on to visit the following projects, which implement a much more robust solution:

http://phototour.cs.washington.edu/bundler/
http://code.google.com/p/libmv/
http://www.cs.washington.edu/homes/ccwu/vsfm/

And Wikipedia points to some interesting libraries and code as well: http://en.wikipedia.org/wiki/Structure_from_motion

Enjoy!

Roy.

The Filter

So I wanted to do a 2D tracker that is more immune to noise. For that I set up a Kalman filter with 4 dynamic parameters and 2 measurement parameters (no control), where my measurement is: 2D location of object, and dynamic is: 2D location and 2D velocity. Pretty simple, and it makes the transition matrix also simple.

KalmanFilter KF(4, 2, 0);
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));

// init...
KF.statePre.at<float>(0) = mouse_info.x;
KF.statePre.at<float>(1) = mouse_info.y;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(.1));

Cool, moving on to the dynamic part.
So I set up a mouse callback to get the mouse position every “frame” (a 100ms wait), and feed that into the filter:

// First predict, to update the internal statePre variable
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

// Get mouse point
measurement(0) = mouse_info.x;
measurement(1) = mouse_info.y;

Point measPt(measurement(0),measurement(1));

// The "correct" phase that is going to use the predicted value and our measurement
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));

All the rest is garnish (see the code)..

The important bit is to see that Predict() happens before Correct(). This is according to the excellent Kalman filter tutorial I found. Look carefully at Figure 1-2!! It will sort you out. Also take a look at OpenCV’s internal impl of Kalman, see that it follows these steps closely. Especially Mat& KalmanFilter::predict(const Mat& control) and Mat& KalmanFilter::correct(const Mat& measurement).
Another good place I found that helped me formulate the parameters for the filter is this place. Again, take everything with a grain of salt, because Kalman Filters are very versatile you just need to know how to formulate them right.

Result

Using velocity:

Not using velocity:

Some Video

Code

As usual, grab the code off the SVN:

svn co http://morethantechnical.googlecode.com/svn/trunk/mouse_kalman/main.cpp

Enjoy,
Roy.

I had. Until recently, I’d accept this as it was from above, but that was until I discovered this website called UnderGet.

The trick is pretty simple. The engine behind this site works by renaming the file or encoding its content so the blocking software cannot detect it.

I am now able to download my favorite podcast mp3 files when I’m at my workplace

So first you’ll need to export your loved tracks from Last.fm in tab separated format – a mere button press.

The “tsv” (tab separated values) file has a simple format: <song name> <artist> <Last.fm url>

And now for the script, first, the loved tracks file is tab separated, so we use AWK to get the 2 first fields which are song-name and song-artist.
Then we use a neat command-line tool to download YT movies: http://rg3.github.com/youtube-dl/documentation.html.

mkdir mylovedtracks
cd mylovedtracks
awk -F\t '{print "../youtube-dl.py -f 18 -t \"ytsearch:" $1 " " $2 "\""}' ../my_lovedtracks.tsv | csh

The single-liner will download all the loved tracks from the tsv file into the current directory, given that youtube-dl.py & my_lovedtracks.tsv exist in the parent directory. -f 18 says it will download only MP4s and ytsearch says it will try to search YT for the term “song-name song-artist” and download the 1st result. The | csh says it will send this command AWK formatted into a new shell process.

The saved MP4 will be named after the name of the video, with addition of the YT hash string.

All the mp4s have been downloaded, so let’s batch convert them to mp3s:

mkdir sound
for f in *.mp4 ; do n=`echo $f | cut -d '.' -f1`; if [ ! -e sound/$n.mp3 ]; then `mplayer $n.mp4 -vc dummy -vo null -ao pcm:file=sound/temp.wav; lame -V2 sound/temp.wav sound/$n.mp3; rm sound/temp.wav`; fi ; done

This single-liner will extract audio from the mp4 into a PCM temp.wav file using MEncoder, and then convert to VBR MP3 using Lame.
You can run this command many times, as it checks if the file has not been converted yet. So you’re impatient (like me) on converting some of the MP4s before everything was downloaded – just run it, and later run it again.

Congrats, all your loved tracks were downloaded.

A few limitation to this method:
* Sometimes downloaded songs are not exactly what you wanted, especially specific versions. The search is arbitrary, and can’t be controlled too much.
* ID3 tags are non existent, although something can probably be done about that in the Lame encoding phase.
* Very high potential for parallelization that is unexploited. Mostly in the YT download phase, where YT pushes the first ~15% of the video very fast (I saw 1200Kb/s even), and then maintains a steady d/l rate to get the video downloaded by ~1:00 minute (may be as low as 50Kb/s). Downloading many videos at once could help.
* Still not a true single-liner, it is a two-step thing. But that can be done by modifying the 2nd step a bit and putting into the AWK print of the 1st step.
* MP3′s volume normalization – very important! else every songs sounds different and you must do vol-up vol-down all the time…

Still, did a nice quick job for me…

Roy.

I believe that every builder-hacker should have their own little Swiss-army-knife server that just does everything they need, but as a webservice. You can basically do anything as a service nowadays: image/audio/video manipulation, mock-cloud data storage, offload heavy computation, and so on.
Tornado, the lightweight Python webserver is perfect for this, and since so many of the projects these days have Python binding (see python-tesseract), it should be a breeze to integrate them with minimal work.
Let’s see how it’s done

Putting it together

I owe the simplicity of this work to the simplicity of Tornado’s API. Really clean, just a couple of entry points to write code.
Since this is an extremely short code, I’ll just pour it in and go over it:

import tornado.httpserver
import tornado.ioloop
import tornado.web
import pprint
import Image
from tesseract import image_to_string
import StringIO

class MainHandler(tornado.web.RequestHandler):
    def get(self):
        self.write('<html><body>Send us a file!<br/><form enctype="multipart/form-data" action="/" method="post">'
                   '<input type="file" name="the_file">'
                   '<input type="submit" value="Submit">'
                   '</form></body></html>')

    def post(self):
        self.set_header("Content-Type", "text/plain")
        self.write("You sent a file with name " + self.request.files.items()[0][1][0]['filename'] )
	# make a "memory file" using StringIO, open with PIL and send to tesseract for OCR
	self.write(image_to_string(Image.open(StringIO.StringIO(self.request.files.items()[0][1][0]['body']))))

application = tornado.web.Application([
    (r"/", MainHandler),
])

if __name__ == "__main__":
    http_server = tornado.httpserver.HTTPServer(application)
    http_server.listen(8888)
    tornado.ioloop.IOLoop.instance().start()

That’s it, and most of it is just garnish. The final version also contains showing the image to the screen.

In the main, Tornado is set up to listen to port 8888, and the application configuration tells it to answer requests on the root (“/”) with our special handler: MainHandler. Then I must define MainHandler to take care of GET and POST requests going in. All this was taken off the “Hello World” of Tornado’s API.

I will have the service answer to POST requests sending an image file, and route it to be processed. All attached files are on self.request.files, so I just pick up the first one.

Now Tesseract, you probably already know, is an open-source OCR engine that was once built by HP and now picked up by Google. It is good as it is free, and has a set of languages already trained.
But I needed a python binding to it, and did not feel like writing one of my own. So I googled and found this small humble project: python-tesseract. With a very narrow API, just a function to call tesseract that basically calls the tesseract command line. But it works like a charm.

So all I needed to do is take the file off the POST request, wrap a StringIO around it to look like a file, use PIL’s Image.open, and send it python-tesseract to return a string. Then I just write the string back to the HTTP response.

	self.write(image_to_string(Image.open(StringIO.StringIO(self.request.files.items()[0][1][0]['body']))))

To get it to actually run you must

• set up $PYTHONPATH variable to find both python-tesseract and Tornado,
• change $TESSDATA_PREFIX to where you put your training data for Tesserast,
• change the path to the tesseract executable in the first code line of python-tesseract’s tesseract.py.

Now all you need is to start your server, send image requests to it, and you’ll get back the text in the images.

Code

Grab it off the SVN:
svn checkout http://morethantechnical.googlecode.com/svn/trunk/tesserver/ tesserver

Enjoy,
Roy.

Just wanted to share a thing I made – a simple 2D hand pose estimator, using a skeleton model fitting. Basically there has been a crap load of work on hand pose estimation, but I was inspired by this ancient work. The problem is setting out to find a good solution, and everything is very hard to understand and implement. In such cases I like to be inspired by a method, and just set out with my own implementation. This way, I understand whats going on, simplify it, and share it with you!

Anyway, let’s get down to business.

A bit about energy minimization problems

A dear friend revealed before me the wonders of energy minimization problems a while back, and ever since I have trying to find uses for that method. Basically, it is trying to find a global minimum for a complicated energy function (usually with many parameters), by following the function’s gradient. Such methods are often called Gradient Descent, and used mostly for non-linear systems that can’t be solved easily using a least-squares variant.

A lot of work in computer vision was done using energy functions (I believe the most seminal was Snakes, over 10,000 citations), usually having two terms: Internal energy and External energy. The equilibrium between the two terms should result in a low-energy system – our optimal result. So we would like to formulate the terms in our system such that when they are 0 – they describe the system as we want it.

Following the works with active contours, I believe the external energy function should have to do with how the hand model fits to the hand blob, and the internal energy will have to do with how “comfortable” the hand is with this configuration.

The hand model

Let’s see how a 2D model of a hand might look like

Kinda looks like a rake… huh?

There are some parts that practically can’t change much, i.e the palm (orange), and some that might change drastically, i.e the fingers (red). Each finger has joints (blue circle), and a tip (bigger blue circle).

typedef struct finger_data {
	Point2d origin_offset;		//base or finger relative to center hand
	double a;					//angle
	vector<double> joints_a;	//angles of joints
	vector<double> joints_d;	//bone length
} FINGER_DATA;

typedef struct hand_data {
	FINGER_DATA fingers[5];		//fingers
	double a;					//angle of whole hand
	Point2d origin;				//center of palm
	Point2d origin_offset;		//offset from center for optimization
	double size;				//relative size of hand = length of a finger
} HAND_DATA;

At first I thought, since I’m only interested in the tips of the fingers, to use Inverse Kinematics to guide the tips to a certain point and let the joints find their own minimal energy position, following this article. But I abandoned this method because of complications.

I also had to simplify this model, for real-time estimation and also better results. So in the end I ended up with a very rigid model, that allows only on joint per finger and no angular movement.

Using tnc.c

tnc.c is a “library”, essentially one c file, that implements a line search algorithm that is able to find the minimum point of a multi-variate function. I’m not certain of the algorithm details, and it’s not so important as it can be replaced with any other similar library. But, tnc.c has a great advantage – it is dead simple. One function will start the gradient decent, calling-back a function to calculate the gradients.

So basically I had to write just one very short function:

static int my_f(double x[], double *f, double g[], void *state) {
	DATA_FOR_TNC* d_ptr = (DATA_FOR_TNC*)state;
	DATA_FOR_TNC new_data = *d_ptr;

	mapVecToData(x,new_data.hand);

	*f = calc_Energy(new_data,*d_ptr);

	//calc gradients
	{
		double _x[SIZE_OF_HAND_DATA];

		for(int i=0;i<SIZE_OF_HAND_DATA;i++) {
			memcpy(_x, x, sizeof(double)*SIZE_OF_HAND_DATA); //reset variables
			_x[i] = _x[i] + EPSILON; //change only one variable
			mapVecToData(_x, new_data.hand);
			double E_epsilon = calc_Energy(new_data,*d_ptr);
			g[i] = ((E_epsilon - *f) / EPSILON); //calc the gradient for this variable change
		}
	}

	return 0;
}

This function is called by tnc.c on every iteration of the search, the double x[] is the state of variables the search is now examining, double* f is the energy for this state, double g[] are the gradients (same size as x[]), and voide* state is a user-defined variable that can be carried along the process.

So what I did is simply changed the value of each parameter in turn, to test how it effects the energy in the system. I get a measure of the energy, then I subtract it from the “natural” setup (without any changes to parameters) energy measure, and I get the gradient for this parameter.

The energy function came out a bit different in the end:

static double calc_Energy(DATA_FOR_TNC& d, DATA_FOR_TNC& orig_d) {
	double _sum = 0.0;

	//external energy: how close are the joints to the hand blob? (how well do they fit to it)
	vector<Point2d> joints;
	Mat tips(5,1,CV_64FC2);

	for (int j=0; j<5; j++) {
		joints.clear();
		FINGER_DATA f = d.hand.fingers[j];
		Point2d _newTip = newTip(f,d.hand,joints); //get joints for this finger

		for (int i=0; i<tmp.size(); i++) { //for each joint find how far it is from the blob
			double ds = pointPolygonTest(d.contour, tmp[i]+getHandOrigin(d.hand), true);
			ds += 5;
			ds = 1 * ((ds < 0) ? -1 : 1) * (ds*ds) ;
			_sum -= (ds > 0) ? 0 : 100*ds;
		}

		tips.at<Point2d>(j,0) = _newTip;
	}

	//lazyness of fingers - joints should strive to be as they were in the natural pose
	vector<double> _angles;
//	for (int j=0; j<5; j++) {
//		FINGER_DATA f = d.hand.fingers[j];
//		FINGER_DATA of = orig_d.hand.fingers[j];
////		_angles.push_back(f.a - of.a);
//		for (int i=0; i<f.joints_d.size(); i++) {
////			_angles.push_back(f.joints_a[i] - of.joints_a[i]);
//			_angles.push_back(f.joints_d[i] - of.joints_d[i]);
//		}
//	}
	_angles.push_back(d.hand.a-orig_d.hand.a); //the angle of the hand should be as it was before
	_sum  += 10000*norm(Mat(_angles));

	if(_sum < 0) return 0;
	return _sum;
}

You’ll notice the commented out section. The “laziness of fingers” turned out not to give good results… A different metric is needed! I have not found it yet, maybe you have a good idea?

Starting tnc.c is very simple: Allocating the vectors for X and gradients, initializing the model from the blob, and calling the simple_tnc convenience method. simple_tnc starts tnc with some default parameters that don’t affect the outcome (at least in my tries).

void estimateHand(Mat& mymask) {
	double _x[SIZE_OF_HAND_DATA] = {0};
	Mat X(1,SIZE_OF_HAND_DATA,CV_64FC1,_x);
	double f;
	Mat gradients(Size(SIZE_OF_HAND_DATA,1),CV_64FC1,Scalar(0));

	namedWindow("state");

	initialize_hand_data(d, mymask);

	mapDataToVec((double*)X.data, d.hand);

	simple_tnc(SIZE_OF_HAND_DATA, (double*)X.data, &f, (double*)gradients.data, my_f, (void*)&d, 1, 0);

	mapVecToData((double*)X.data, d.hand);
	showstate(d,1);

	d.hand.origin = getHandOrigin(d.hand); //move to new position
}

Results and Discussion

Here are my results so far:

It’s not perfect, but it’s a start. Tracking and estimating open hand is pretty good, with some orientation change as well. But when the fingers are closed… that’s where problems start.

Sometimes the joints “hover” over the black area to “land” in a white area so they “fit”, but they should not do that. One easy thing to do to counter this is to measure the distance of the whole bone, and not just the joint.

The model right now doesn’t use all the joints possible, because it is too heavy computationally. Plus the energy does not depend (or change) the angle of the fingers. So this is a very very simple model of a hand…

But, it is a good start! All the other stuff I have seen online is just basic high-curvature points counting and color-based or feature-based segmentation and tracking… My model actually tries to fit an articulate and precise model of a hand to the image.

How did you get such nice blobs?!

You ask. They are beautiful aren’t they… nice and clean, easy for tracking and model fitting. It’s no magic though…
Well, I took part of a project in the Media Lab, called DepthJS, that uses the MS Kinect to control web pages. I wrote the computer-vision part. So all the code is there, you can grab it, I just plugged it into this little project. Basing off this very simple example of using OpenCV2.X and libfreenect.

Wow, this was a longie.. I hope you learned something and got inspired. I got to do a second overview of the project, and I’m inspired. Inspiration all around!

Code is obviously yours for the taking:
https://github.com/royshil/OpenHPE

Please contribute your own views, thoughts, code, rants in the comments and github page.

Enjoy
Roy.

I was inspired by Lior Shapira’s work that was presented in Eurographics 09 about image appearance manipulation, and a work about recoloring for the colorblind by Huang et al presented at ICASSP 09. Both works deal with color manipulation using Gaussian Mixture Models.

Let’s see how it’s done!

A little theory

I won’t bore you with the math, but just to get a hang of the idea, what we would like to do is learn how the colors in the source and target images are distributed. Naturally we can assume, like many other things in nature, the colors in a picture have a normal (Gaussian) distribution, but we can go further by saying the distribution might have a few Gaussians describing it. This is called a mixture distribution, and it’s a very handy statistical tool. Mixtures can be estimated using a powerful and ubiquitous tool called Expectation Maximization, which I have previously covered. EM essentially tries to recover the mean (mu) and variance (sigma) of the Gaussians in the mixture, by iteratively checking the current hypothesis against the data until finally converging at an extremum.

Learning the color model

For the learning process we must set up the sample data. So we create a binary mask saying where in the image the model can find the colors to learn.
–images–
Then we scan the mask and for each foreground pixel we add it’s value (here, RGB, but basically can be anything) to the sample data. Finally we train the CvEM object, which contains the GMM parameters.

void TrainGMM(CvEM& source_model, Mat& source, Mat& source_mask) {
		int src_samples_size = countNonZero(source_mask);
		Mat source_samples(src_samples_size,3,CV_32FC1);

		Mat source_32f;
		source_32f = source;

		int sample_count = 0;
		for(int y=0;y<source.rows;y++) {
			Vec3f* row = source_32f.ptr<Vec3f>(y);  //pointer to pixel data in the row
			uchar* mask_row = source_mask.ptr<uchar>(y); //pointer to binary mask
			for(int x=0;x<source.cols;x++) {
				if(mask_row[x] > 0) {
					source_samples.at<Vec3f>(sample_count++,0) = row[x];
				}
			}
		}

		source_model.clear();
		CvEMParams ps(3/* = number of gaussians*/);
		source_model.train(source_samples,Mat(),ps,NULL);
}

What we have are three 3-dimensional (R,G,B) Gaussians, describing the colors in the selected area.

Matching Gaussians

Now we have a couple of GMMs – one for the target and one for the source. The idea is to take a pixel in the target, see how the 3 target Gaussians describe it, and shift it to use the 3 source Gaussians. This will, hopefully, cause its color to change from target to source. But we need to know which Gaussian in the target corresponds to which Gaussian in the source. I made a quick selection algorithm that greedily chooses a Gaussian for each Gaussian. I permutate the order of selection for the greedy algorithm, and pick the best permutation to get closer to the optimal selection.

vector<int> Recoloring::MatchGaussians(CvEM& source_model, CvEM& target_model) {
		int num_g = source_model.get_nclusters();
		Mat sMu(source_model.get_means());
		Mat tMu(target_model.get_means());
		const CvMat** target_covs = target_model.get_covs();
		const CvMat** source_covs = source_model.get_covs();

		double best_dist = std::numeric_limits<double>::max();
		vector<int> best_res(num_g);
		vector<int> prmt(num_g); 

		for(int itr = 0; itr < 10; itr++) {
			for(int i=0;i<num_g;i++) prmt[i] = i;	//make a permutation
			randShuffle(Mat(prmt));

			//Greedy selection
			vector<int> res(num_g);
			vector<bool> taken(num_g);
			for(int sg = 0; sg < num_g; sg++) {
				double min_dist = std::numeric_limits<double>::max();
				int minv = -1;
				for(int tg = 0; tg < num_g; tg++) {
					if(taken[tg]) continue;

					//TODO: can save on re-calculation of pairs - calculate affinity matrix ahead
					//double d = norm(sMu(Range(prmt[sg],prmt[sg]+1),Range(0,3)),	tMu(Range(tg,tg+1),Range(0,3)));

					//symmetric kullback-leibler
					Mat diff = Mat(sMu(Range(prmt[sg],prmt[sg]+1),Range(0,3)) - tMu(Range(tg,tg+1),Range(0,3)));
					Mat d = diff * Mat(Mat(source_covs[prmt[sg]]).inv() + Mat(target_covs[tg]).inv()) * diff.t();
					Scalar tr = trace(Mat(
						Mat(Mat(source_covs[prmt[sg]])*Mat(target_covs[tg])) +
						Mat(Mat(target_covs[tg])*Mat(source_covs[prmt[sg]]).inv()) +
						Mat(Mat::eye(3,3,CV_64FC1)*2)
						));
					double kl_dist = ((double*)d.data)[0] + tr[0];
					if(kl_dist<min_dist) {
						min_dist = kl_dist;
						minv = tg;
					}
				}
				res[prmt[sg]] = minv;
				taken[minv] = true;
			}

                       //total distance for the permutation
			double dist = 0;
			for(int i=0;i<num_g;i++) {
				dist += norm(sMu(Range(prmt[i],prmt[i]+1),Range(0,3)),
							tMu(Range(res[prmt[i]],res[prmt[i]]+1),Range(0,3)));
			}
			if(dist < best_dist) {
				best_dist = dist;
				best_res = res;
			}
		}

		return best_res;
	}

I used Symmetric Kullback-Leibler for the distance between Gaussians, as suggested by Huang et al.

Applying the color

Now all we have to do is use the method Shapira suggested in his work to transform a pixel’s color, from the Gaussians describing it.
I’m only putting the essence of the code, the rest is in the file.

		Mat pr; Mat samp(1,3,CV_32FC1);
		for(int y=0;y<target.rows;y++) {
			Vec3f* row = target_32f.ptr<Vec3f>(y);
			uchar* mask_row = target_mask.ptr<uchar>(y);
			for(int x=0;x<target.cols;x++) {
				if(mask_row[x] > 0) {
                                        //take pixel data
					memcpy(samp.data,&(row[x][0]),3*sizeof(float)); 

                                        //Use the GMM to predict how close this pixel is to each gaussian
					float res = target_model.predict(samp,&pr);

					Mat samp_64f; samp.convertTo(samp_64f,CV_64F);

                                        //Move the pixel to the new Gaussians
					//From Shapira09: Xnew = Sum_i { pr(i) * Sigma_source_i * (Sigma_target_i)^-1 * (x - mu_target) + mu_source }
					Mat Xnew(1,3,CV_64FC1,Scalar(0));
					for(int i=0;i<num_g;i++) {
						if(((float*)pr.data)[i] <= 0) continue;

                                               //For each Gaussian, subtract the original mean and add the target mean,
                                               //use probabilities to get a weighted average.
						Xnew += Mat((
							//Mat(source_covs[match[i]]) *
							//Mat(target_covs[i]).inv() *
							Mat(samp_64f - tMu_64f(Range(i,i+1),Range(0,3))).t() +
							sMu_64f(Range(match[i],match[i]+1),Range(0,3)).t()
							) * (double)(((float*)pr.data)[i])).t();
					}

                                        //Put pixel back into place
					Mat _tmp; Xnew.convertTo(_tmp,CV_32F);
					memcpy(&(row[x][0]),_tmp.data,sizeof(float)*3);
				}
			}
		}

You might notice I skip the part of multiplying by the covariances matrices, as Shapira did. I found it produces better results, but it’s probably caused by a bug.

Results

Code and stuff

Source code is available in SVN repo:

svn checkout http://morethantechnical.googlecode.com/svn/trunk/GMM_Recoloring recoloring

Images from Flickr (Creative Commons):

• http://www.flickr.com/photos/wwworks/2956622857/sizes/s/
• http://www.flickr.com/photos/violentz/3199292482/sizes/s/
• http://www.flickr.com/photos/davidw/164670455/sizes/m/
• http://www.flickr.com/photos/djania/252225693/sizes/m/

Now go recolor the world!

Thanks for tuning in..
Roy.

Been wokring on my masters thesis for a while now, and the path of my work came across image segmentation. Naturally I became interested in Max-Flow Graph Cuts algorithms, being the “hottest fish in the fish-market” right now if the fish market was the image segmentation scene.

So I went looking for a CPP implementation of graphcut, only to find out that OpenCV already implemented it in v2.0 as part of their GrabCut impl. But I wanted to explore a bit, so I found this implementation by Olga Vexler, which is build upon Kolmogorov’s framework for max-flow algorithms. I was also inspired by Shai Bagon’s usage example of this implementation for Matlab.

Let’s jump in…

Bit of Theory

Before we move on, let’s dig in a little in the theory. We look at the picture as a set of nodes, where each pixel is node and is connected to its neighbors by edges and has a label – this can be called a Markov Random Field. MRFs can be solved, i.e. give an optimal labeling for each node and thus an optimal labeling, in a number of ways, one of which being graph cuts based on maximal flow. After we label the graph, we expect to get a meaningful segmentation of the image. This paper, by some of the big names in the field (Vexler, Kolmogorov, Agarwala), explains it pretty throughly. There a number of well known segmentation methods that use graph cuts, such as: Lazy Snapping [04], GrabCut [04] and more.

The math in the articles is, as usual, pretty horrific. I like to keep things simple, so I’ll try to explain the method of GC-segmentation in a simple way. We all remember min cut – max flow algorithms from 2nd year CS, right? well segmentation using GC is not very different. The magic happens when we weight the nodes and edges in a meaningful way, thus creating meaningful cuts. The weights are usually spit to two terms: Data term (or cost) and Smoothness term. The data term says in simple words: “How happy this pixels is with that label”, and the smoothness term pretty much says “How easy can a label expand from this pixel to that neighbor”. So when you think about it, the easiest thing would be to put as the data term the likelyhood of a pixel to belong to some label, and for the smoothness term – just use the edges in the picture!

So anyway, back to the code, only thing left is to create a graph, give weights, and max the flow. Here’s a bit of code:

Mat im_small = imread("a_pic.jpg");
int num_lables = 3;

// create a grid type graph
GCoptimizationGridGraph gc(im_small.cols,im_small.rows,num_lables);

This piece of code created a directed grid graph where every pixel will be a vertex, and each pixel can have one of 3 lables (3 parts in the image to segment).

GMM to the rescue!

Now for the weighting. One very “standard” way to give a data term to the pixels is by using Gaussian-Mixture-Models (GMM): A method that fits a few gaussian distributions over an unknown probability function to estimate how it looks. In the spirit of keeping things simple, I won’t go into details. I’ll just say that it’s a tool to get the probablility of a pixel to belong to a cluster of other pixels, and it has built-in implementation in OpenCV, which is reason enough for me to use it. In OpenCV GMM models are named EM, which is kind of erroneous, since EM (Expectation-Maximization) is one of the best methods to estimate a GMM and not a GMM itself.

Using EM in OpenCV is really very easy:

CvEM model;
CvEMParams ps(3);

Mat lables;
Mat samples;
im.reshape(1,im.rows*im.cols).convertTo(samples,CV_32FC1,1.0/255.0);

model.train(samples,Mat(),ps,&lables);

Mat probs = model.get_probs();

Here’s how it looks when training over the whole image as input data (you can see original image, labeling, minus log probability):

Clustering using EM in OpenCV

But, this is not exactly what we wanted… Since we are dealing with segmentation here, we would like to segment certain area. The purpose of the GMM is to learn how that area looks, based on a small set of samples, and then predict the label for all the pixels in the image.

In GrabCut they pretty much create a GMM for every logical “cluster” they want to segment: Positively Background, Probably Background, Probably Foreground and Positively Foreground. This is a good idea and I will follow it, but again, I’m aiming not for Object Extraction rather for k-way segmentation. In other words I’m looking for a way to divide the image to a few areas that are significantly similar, and also not similar to the other areas.

To do that I will create a K-gaussians GMM for N areas (see the code, it’s a long one). I tried 2 versions, where I create a 1-gaussian GMM for each channel (RGB, etc.) of each area it’s called doEM1D(), and another one with K-gaussian and N-clusters GMM for each area. The results have varied:

Three 1 channel 1 Gaussian GMMs

Three 3-channels 3-Gaussians per channel GMMs

This will provide us the data-term for our segmentation – each pixel can now say how comfortable it is with the label it got (we simply use the probability from the GMM).

Play it smooth

Right, moving on to the smoothness term. I mentioned before it would be easiest to just use the edges in the image. I use the Sobel filter, which gives a nice strong edge. Again we must look at each pixel’s value as the likelyhood to have an edge in it, so we should use -log to get it in nice big integers where the probability drops.

GaussianBlur(gray32f,gray32f,Size(11,11),0.75);

Sobel(gray32f,_tmp,-1,1,0,3);	//sobel for dx
_tmp1 = abs(_tmp);  //use abs value to get also the opposite direction edges
_tmp1.copyTo(res,(_tmp1 > 0.2));  //threshold the small edges...

double maxVal,minVal;
minMaxLoc(_tmp,&minVal,&maxVal);
cv::log(res,_tmp);
_tmp = -_tmp * 0.17;
_tmp.convertTo(grayInt1,CV_32SC1);

Sobel(gray32f,_tmp,-1,0,1,3);	//sobel for dy
_tmp1 = abs(_tmp);
res.setTo(Scalar(0));
_tmp1.copyTo(res,(_tmp1 > 0.2));

imshow("tmp1",res); waitKey();

minMaxLoc(_tmp,&minVal,&maxVal);
cv::log(res,_tmp);
_tmp = -_tmp * 0.17;
_tmp.convertTo(grayInt,CV_32SC1);

Now put everything into a bowl and mix!

And the last part of the process will be to put the labels probabilities per pixel and edges into the grid graph we created earlier:

GCoptimizationGridGraph gc(im.cols,im.rows,num_lables);

//Set the pixel-label probability
int N = im.cols*im.rows;
double log_1_3 = log(1.3);
for(int i=0;i<N;i++) {
   double* ppt = probs.ptr<double>(i);
   for(int l=0;l<num_lables;l++) {
      int icost = MAX(0,(int)floor(-log(ppt[l])/log2));
      gc.setDataCost(i,l,icost);
   }
}

//Set the smoothness cost
Mat Sc = 5 * (Mat::ones(num_lables,num_lables,CV_32SC1) - Mat::eye(num_lables,num_lables,CV_32SC1));
gc.setSmoothCostVH((int*)(Sc.data),(int*)dx.data,(int*)dy.data);

lables.create(N,1,CV_8UC1);

printf("\nBefore optimization energy is %d\n",gc.compute_energy());
gc.expansion(1);
printf("\nAfter optimization energy is %d\n",gc.compute_energy());

//Get the labeling back from the graph
for ( int  i = 0; i < N; i++ )
   ((uchar*)(lables.data + lables.step * i))[0] = gc.whatLabel(i);

Easy. Now the labeling should give us a nice segmentation:

3 x 3-ch-3-gs GMMs labeling

3 x 1-ch-1-gs GMMs labeling

But, there’s a lot of noise in the labeling… A good heuristic to apply will be to take only the largest connected-component of each label, and also try to the the component that is closest to the original marking.

Lables extraction without larget component keeping

Lables extraction with largest component keeping

vector<vector<Point>> contours;
for(int itr=0;itr<2;itr++) {

Mat mask = (_tmpLabels == itr); //Get a mask of this label

contours.clear();
//find the contours in that mask
cv::findContours(mask,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);

//compute areas
vector<double> areas(contours.size());
for(unsigned int ai=0;ai<contours.size();ai++) {
	Mat _pts(contours[ai]);
	Scalar mp = mean(_pts);

	areas[ai] = contourArea(Mat(contours[ai]))/* add some bias here to get components that are closer to initial marking*/;
}

//find largest connected component
double max; Point maxLoc;
minMaxLoc(Mat(areas),0,&max,0,&maxLoc);

//draw back on mask
_tmpLabels.setTo(Scalar(3),mask);	//all unassigned pixels will have value of 3, later we'll turn them to "background" pixels

mask.setTo(Scalar(0)); //clear...
drawContours(mask,contours,maxLoc.y,Scalar(255),CV_FILLED);

//now that the mask has only the wanted component...
_tmpLabels.setTo(Scalar(itr),mask);

}

Code and salutations

As usual the code is available from the blog’s SVN:

svn checkout http://morethantechnical.googlecode.com/svn/trunk/GMMGraphCutSegmentation GMMGraphCutSegmentation

Hey! We’re pretty much done! Glad you (and I) made it to the end, it wasn’t easy after all… I hope you learned something about GMMs and Graph-Cuts in OpenCV and in general.

BTW: The pictures are from Flickr, under creative commons license.
http://farm1.static.flickr.com/33/40406598_fd4e74d51c_d.jpg
http://www.flickr.com/photos/willemvelthoven/56589010/sizes/m/in/pool-99557785@N00/

See ya!
Roy.

Just wanted to share a small thing I did with OpenCV – Head Pose Estimation (sometimes known as Gaze Direction Estimation). Many people try to achieve this and there are a ton of papers covering it, including a recent overview of almost all known methods.

I implemented a very quick & dirty solution based on OpenCV’s internal methods that produced surprising results (I expected it to fail), so I decided to share. It is based on 3D-2D point correspondence and then fitting of the points to the 3D model. OpenCV provides a magical method – solvePnP – that does this, given some calibration parameters that I completely disregarded.

Here’s how it’s done

Intro

I wanted to use solvePnP, since I saw how easy it was to use it when I was implementing the PTAM. It’s supposed to recover the 3D location and orientation of an object, given a 3D-2D feature correspondence and an initial guess. In fact the initial guess is not required, but the results when not using the guess are dreadful.

So I needed to get some 3D points on a human head. I downloaded a free model of a human head from the net, and used MeshLab to mark some points on the model:

1. Left ear
2. Right ear
3. Left eye
4. Right eye
5. Nose tip
6. Left mouth corner
7. Right mouth corner

Then I headed to LFW database to get some pictures of celebrity heads. By mere accident I stumbled upon Angelina Jolie. The next step was to mark some points on Angelina’s pictures, according to the selected features. In places where the head hides an ear, I put a point in the estimated location of the ear.

Time to Code

First I initialize the 3D points vector, and a dummy camera matrix:

vector<Point3f > modelPoints;
modelPoints.push_back(Point3f(-36.9522f,39.3518f,47.1217f));    //l eye
modelPoints.push_back(Point3f(35.446f,38.4345f,47.6468f));              //r eye
modelPoints.push_back(Point3f(-0.0697709f,18.6015f,87.9695f)); //nose
modelPoints.push_back(Point3f(-27.6439f,-29.6388f,73.8551f));   //l mouth
modelPoints.push_back(Point3f(28.7793f,-29.2935f,72.7329f));    //r mouth
modelPoints.push_back(Point3f(-87.2155f,15.5829f,-45.1352f));   //l ear
modelPoints.push_back(Point3f(85.8383f,14.9023f,-46.3169f));    //r ear

op = Mat(modelPoints);
op = op / 35; //just a little normalization...
rvec = Mat(rv);
double _d[9] = {1,0,0,
          0,-1,0,
         0,0,-1}; //rotation: looking at -z axis
Rodrigues(Mat(3,3,CV_64FC1,_d),rvec);
tv[0]=0;tv[1]=0;tv[2]=1;
tvec = Mat(tv);
double _cm[9] = { 20, 0, 160,
           0, 20, 120,
             0,  0,   1 };  //"calibration matrix": center point at center of picture with 20 focal length.
camMatrix = Mat(3,3,CV_64FC1,_cm);

Even though the “calibration” parameters are totally bogus they work pretty good.

Now, we’re all ready to start estimating some poses. So let’s use solvePnP:

vector<Point2f > imagePoints;

//read 2D points from file...
FILE* f;
fopen_s(&f,"points.txt","r");
for(int i=0;i<7;i++) {
     int x,y;
     fscanf_s(f,"%d",&x); fscanf_s(f,"%d",&y);
     imagePoints.push_back(Point2f((float)x,(float)y));
}
fclose(f);</td>

//make a Mat of the vector<>
Mat ip(imagePoints);

//display points on image
Mat img = imread("image.png");
for(unsigned int i=0;i<imagePoints.size();i++) circle(img,imagePoints[i],2,Scalar(255,0,255),CV_FILLED);

//"distortion coefficients"... hah!
double _dc[] = {0,0,0,0};

//here's where the magic happens
solvePnP(op,ip,camMatrix,Mat(1,4,CV_64FC1,_dc),rvec,tvec,true);

//decompose the response to something OpenGL would understand.
//translation vector is irrelevant, only rotation vector is important
Mat rotM(3,3,CV_64FC1,rot);
Rodrigues(rvec,rotM);
double* _r = rotM.ptr<double>();
printf("rotation mat: \n %.3f %.3f %.3f\n%.3f %.3f %.3f\n%.3f %.3f %.3f\n",
          _r[0],_r[1],_r[2],_r[3],_r[4],_r[5],_r[6],_r[7],_r[8]);

Alright, all done on the vision side, so I draw some 3D. As usual, I use a very simple GLUT program to display 3D in a hurry. Initialization is nothing special, so just one thing I think is special is using glutSoldCylinder and glutSolidTetrahedron to draw the axes:

glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0,0,0,0,0,1,0,1,0); //cam looking at +z axis
glPushMatrix();
glTranslated(0,0,5); //go a bit back to where I want to draw the axes
glPushMatrix();

//this is the rotation matrix I got from solvePnP, so I will rotate accordingly to align with the face
double _d[16] = {       rot[0],rot[1],rot[2],0,
                rot[3],rot[4],rot[5],0,
                rot[6],rot[7],rot[8],0,
                0,         0,     0             ,1};
glMultMatrixd(_d);
glRotated(180,1,0,0); //rotate around to face the camera

//----------- Draw Axes --------------
//Z = red
glPushMatrix();
glRotated(180,0,1,0);
glColor3d(1,0,0);
glutSolidCylinder(0.05,1,15,20);
glTranslated(0,0,1);
glScaled(.1,.1,.1);
glutSolidTetrahedron();
glPopMatrix();

//Y = green
glPushMatrix();
glRotated(-90,1,0,0);
glColor3d(0,1,0);
glutSolidCylinder(0.05,1,15,20);
glTranslated(0,0,1);
glScaled(.1,.1,.1);
glutSolidTetrahedron();
glPopMatrix();

//X = blue
glPushMatrix();
glRotated(-90,0,1,0);
glColor3d(0,0,1);
glutSolidCylinder(0.05,1,15,20);
glTranslated(0,0,1);
glScaled(.1,.1,.1);
glutSolidTetrahedron();
glPopMatrix();

glPopMatrix();
glPopMatrix();
//----------End axes --------------

That wasn’t too hard, huh? Awesome.

So…. Results

Code

You can grab the code from the SVN repo:

svn checkout http://morethantechnical.googlecode.com/svn/trunk/HeadPose

Enjoy!

Roy.

Been working hard at a project for school the past month, implementing one of the more interesting works I’ve seen in the AR arena: Parallel Tracking and Mapping (PTAM) [PDF]. This is a work by George Klein [homepage] and David Murray from Oxford university, presented in ISMAR 2007.

When I first saw it on youtube [link] I immediately saw the immense potential – mobile markerless augmented reality. I thought I should get to know this work a bit more closely, so I chose to implement it as a part of advanced computer vision course, given by Dr. Lior Wolf [link] at TAU.

The work is very extensive, and clearly is a result of deep research in the field, so I set to achieve a few selected features: Stereo initialization, Tracking, and small map upkeeping. I chose not to implement relocalization and full map handling.

This post is kind of a tutorial for 3D reconstruction with OpenCV 2.0. I will show practical use of the functions in cvtriangulation.cpp, which are not documented and in fact incomplete. Furthermore I’ll show how to easily combine OpenCV and OpenGL for 3D augmentations, a thing which is only briefly described in the docs or online.

Here are the step I took and things I learned in the process of implementing the work.

Update: A nice patch by yazor fixes the video mismatching – thanks! and also a nice application by Zentium called “iKat” is doing some kick-ass mobile markerless augmented reality.

Preparations…

Before going straight to coding, I had to prepare a few things.

• A working compilation of OpenCV – not trivial with the new version 2.0.
• A calibrated camera.
• Test data

Compiling OpenCV 2.0 proved to be a bit tricky. Even though the sourceforge project offers binary release for Win32, I compiled the whole thing from source. It turned out the binary release doesn’t contain .lib files, and anyway has compatibility issues between MS VS 2005 and 2008 – something about the embedded manifest [google]. I downloaded the freshest source from SVN, and compiled it, but it didn’t solve the debug-release problem, so I was left with using the release dlls even for debug evironment.

Initially I thought I’ll try an uncalibrated camera approach, but soon abandoned it. I had to calibrate my cameras, which I did  very easily using OpenCV’s “calibration.cpp”, which strangely is not built when building all examples – it has to be built manually. But everything went smoothly, and I soon got a calibration matrix (focal length, center of projection) and radial distortion coefficients.

Getting Test Data

For the test data I wanted to get a few views of a planar scene, where the first two views are separated only by a translation of ~5cm, as K&M do in the PTAM article. This known translation is helpful when trying to triangulate the initial features in the scene. When you have prior knowledge of where the cameras are, you can simply intersect the epipolar lines between the two views and recover the 3D position of the points – up to a scale. Keep in mind you must also have feature correspondence: a point on image A must be correlated to a point in image B.

To achieve this I set up a small program that uses Optical Flow to track some 2D features in the scene, and grab a few screens + feature vectors. See ‘capture_data.cpp’.

Stereo Initialization

Now that I have 2 views with feature correspodence:

I would like to triangulate the features. This is possible, as I discussed earlier, since I know the rotation (none), translation (5cm on -x axis) and camera calibration parameters (focal length, center of projection).

Triangulation

For triangulation, OpenCV has only recently added a couple of functions that implement triangulation [link] as shown by Hartly & Zisserman [PDF, page 12]. However, these functions are not formally documented, and in fact they are missing some important parts. This is how I used cvTriangulation(), which is the key function:

//this function will initialize the 3D features from two views
void stereoInit() {

//first load camera intrinsic parameters
FileStorage fs("cam_work.out",CV_STORAGE_READ);
FileNode fn = fs["camera_matrix"];
camera_matrix = Mat((CvMat*)fn.readObj(),true);

fn = fs["distortion_coefficients"];
distortion_coefficients = Mat((CvMat*)fn.readObj(),true);

//vector<Point2d> points[2]; //these Point2d vectors hold the 2D features, double precision, from the 2 views

//get copy of points
_points[0] = points[0];
_points[1] = points[1];
Mat pts1M(_points[0]), pts2M(_points[1]); //very easy in OpenCV 2.0 to convert vector<> to Mat.

//Undistort points
Mat tmp,tmpOut;
pts1M.convertTo(tmp,CV_32FC2);  //undistort takes only floats not doubles, so convert to Point2f
undistortPoints(tmp,tmpOut,camera_matrix,distortion_coefficients);
tmpOut.convertTo(pts1M,CV_64FC2);  //go back to double precision

pts2M.convertTo(tmp,CV_32FC2);
undistortPoints(tmp,tmpOut,camera_matrix,distortion_coefficients);
tmpOut.convertTo(pts2M,CV_64FC2);

vector<uchar> tri_status; //this will hold the status for each point, a good point will have 1, bad - 0

//now triangulate
triangulate(_points[0],_points[1],tri_status);

}

void triangulate(vector<Point2d>& points1, vector<Point2d>& points2, vector<uchar>& status) {

	//Convert points to 1-channel, 2-rows, double precision - This is important - see the code
...

	Mat ___tmp(2,pts1Mt.cols,CV_64FC1,__d);
...
	Mat ___tmp1(2,pts2Mt.cols,CV_64FC1,__d1);
...

	CvMat __points1 = ___tmp, __points2 = ___tmp1;

	//projection matrices
	double P1d[12] = {	-1,0,0,0,
						0,1,0,0,
						0,0,1,0 };	//Identity, but looking into -z axis
	Mat P1m(3,4,CV_64FC1,P1d);
	CvMat* P1 = &(CvMat)P1m;
	double P2d[12] = {	-1,0,0,-5,
						0,1,0,0,
						0,0,1,0 };  //Identity rotation, 5cm -x translation, looking into -z axis
	Mat P2m(3,4,CV_64FC1,P2d);
	CvMat* P2 = &(CvMat)P2m;

	float _d[1000] = {0.0f};
	Mat outTM(4,points1.size(),CV_32FC1,_d);
	CvMat* out = &(CvMat)outTM;

//using cvTriangulate with the created structures
	cvTriangulatePoints(P1,P2,&__points1,&__points2,out);

//we should check the triangulation result by reprojecting 3D->2D and checking distance
	vector<Point2d> projPoints[2] = {points1,points2};

	double point2D_dat[3] = {0};
	double point3D_dat[4] = {0};
	Mat twoD(3,1,CV_64FC1,point2D_dat);
	Mat threeD(4,1,CV_64FC1,point3D_dat);

	Mat P[2] = {Mat(P1),Mat(P2)};

	int oc = out->cols, oc2 = out->cols*2, oc3 = out->cols*3;

	status = vector<uchar>(oc);

	//scan all points, reproject 3D->2D, and keep only good ones
	for(int i=0;i<oc;i++) {
		double W = out->data.fl[i+oc3];
        point3D_dat[0] = out->data.fl[i] / W;
        point3D_dat[1] = out->data.fl[i+oc] / W;
        point3D_dat[2] = out->data.fl[i+oc2] / W;
        point3D_dat[3] = 1;

        bool push = true;
        /* !!! Project this point for each camera */
        for( int currCamera = 0; currCamera < 2; currCamera++ )
        {
            //reproject! using the P matrix of the current camera
			twoD = P[currCamera] * threeD;

            float x,y;
            float xr,yr,wr;
 	x = (float)projPoints[currCamera][i].x;
	y = (float)projPoints[currCamera][i].y;

            wr = (float)point2D_dat[2];
            xr = (float)(point2D_dat[0]/wr);
            yr = (float)(point2D_dat[1]/wr);

            float deltaX,deltaY;
            deltaX = (float)fabs(x-xr);
            deltaY = (float)fabs(y-yr);

			//printf("error from cam %d (%.2f,%.2f): %.6f %.6f\n",currCamera,x,y,deltaX,deltaY);

			if(deltaX > 0.01 || deltaY > 0.01) {
				push = false;
			}
        }
		if(push) {
			// A good 3D reconstructed point, add to known world points

			double s = 7;
			Point3d p3d(point3D_dat[0]/s,point3D_dat[1]/s,point3D_dat[2]/s);
			//printf("%.3f %.3f %.3f\n",p3d.x,p3d.y,p3d.z);
			points1Proj.push_back(p3d);
			status[i] = 1;
		} else {
			status[i] = 0;
		}

	}
}

OK, now that I have (hopefully) triangulated 3D features from the initial state: 2 views of a planar scene with 5cm translation on the X axis – I can move on the pose estimation.

Pose Estimation

Theoretically, if I know the 3D position of features in the world and their respective 2D position in the image, it should be easy to recover the position of the camera, because there are a rotation matrix and translation vector that define this transformation. Practically in OpenCV, finding the position of an object using 3D-2D correlation is done by using the solvePnP() [link] function.

Since I have an initial guess of the rotation and translation – from the first 2 frames – I can “help” the function estimate the new ones.

void findExtrinsics(vector<Point2d>& points, vector<double>& rv, vector<double>& tv) {
	//estimate extrinsics for these points

	Mat rvec(rv),tvec(tv);

//initial "guess", in case it wasn't already supplied
	if(rv.size()!=3) {
		rv = vector<double>(3);
		rvec = Mat(rv);
		double _d[9] = {1,0,0,
						0,-1,0,
						0,0,-1};
		Rodrigues(Mat(3,3,CV_64FC1,_d),rvec);
	}
	if(tv.size()!=3) {
		tv = vector<double>(3);
		tv[0]=0;tv[1]=0;tv[2]=0;
		tvec = Mat(tv);
	}

	//create a float rep  of points
	vector<Point2f> v2(points.size());
	Mat tmpOut(v2);
	Mat _tmpOut(points);
	_tmpOut.convertTo(tmpOut,CV_32FC2);

	solvePnP(points1projMF,tmpOut,camera_matrix,distortion_coefficients,rvec,tvec,true);

	printf("frame extrinsic:\nrvec: %.3f %.3f %.3f\ntvec: %.3f %.3f %.3f\n",rv[0],rv[1],rv[2],tv[0],tv[1],tv[2]);

//the output of the function is a Rodrigues form of rotation, so convert to regular rot-matrix
	Mat rotM(3,3,CV_64FC1); ///,_r);
	Rodrigues(rvec,rotM);
	double* _r = rotM.ptr<double>();
	printf("rotation mat: \n %.3f %.3f %.3f\n%.3f %.3f %.3f\n%.3f %.3f %.3f\n",
		_r[0],_r[1],_r[2],_r[3],_r[4],_r[5],_r[6],_r[7],_r[8]);
}

After getting the extrinsic parameters of the camera, the next step is plugging in the visualization!

Integrating OpenGL

Generally, it should be possible to create a 3D scene that matches exactly the true world scene, where the triangulated features appear in the scene aligned exactly with the world. I was not able to achieve that, but I got pretty close:

It’s basically what you do in augmented reality, you align the virtual camera’s position and rotation with the results you get from the vision part of the system. In the pose estimation we ended with a 3D rotation vector (Rodrigues form) and 3D translation vector which is used as-is, so only the rotation vector should be converted to 3×3 matrix using the Rodrigues() function.

This is the OpenGL glut display() function that draws the scene:

void display(void)
{
	glClearColor(1.0f, 1.0f, 1.0f, 0.5f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer

	//draw the background - the frame from the camers
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	gluOrtho2D(0.0,352.0,288.0,0.0);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glDisable(GL_DEPTH_TEST);
	glDrawPixels(352,288,GL_RGB,GL_UNSIGNED_BYTE,backPxls.data);
	glEnable(GL_DEPTH_TEST);
	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();

    const double t = glutGet(GLUT_ELAPSED_TIME) / 1000.0;
	a = t*20.0;

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

//use the camera position 3D vector
	curCam[0] = cam[0]; curCam[1] = cam[1]; curCam[2] = cam[2];
//there seems to be some kind of offset...
	glTranslated(-curCam[0]+0.5,-curCam[1]+0.7,-curCam[2]);

//and the 3x3 rotation matrix
	double _d[16] = {	rot[0],rot[1],rot[2],0,
						rot[3],rot[4],rot[5],0,
						rot[6],rot[7],rot[8],0,
						0,	   0,	  0		,1};
	glMultMatrixd(_d);

//flip the rotation on the x-axis
	glRotated(180,1,0,0);

	//draw the 3D feature points
	glPushMatrix();
	glColor4d(1.0,0.0,0.0,1.0);
	for(unsigned int i=0;i<points1Proj.size();i++) {
		glPushMatrix();
glTranslated(points1Proj[i].x,points1Proj[i].y,points1Proj[i].z);
		glutSolidSphere(0.03,15,15);
		glPopMatrix();
	}
	glPopMatrix();

	glutSwapBuffers();

	if(!running) {
		glutLeaveMainLoop();
	}

	Sleep(25);
}

This pretty much coveres my work, in a very concise way. The complete source code will reveal all I have done, and will provide a better copy-and-paste ground for your own projects.

Things not covered in this work

Initially I tried to implement a very crucial part of the PTAM work – pairing the 3D map with 2D features in the image. This allows them to re-align the map in every frame (when the tracking is bad) so the pose estimation does not “loose grip”. In essence, they keep a visual identity for each map feature, very similar to a descriptor like SURF or SIFT, so at any point they can find where in the new image are the features and recover the camera pose from the 2D-3D correspondence. I ran into a problem utilizing OpenCV’s SURF functionality, it seems to have a bug when trying to compute the descriptor for user-given feature points.

Another thing I chose not to implement is creating a full map of the surroundings. I wanted to achieve a simple working solution for a small map (essentially a single frame), and see how it works. In the original work by K&M they constantly add more and more features to the map untill it has covered the whole surrounding room.

Code and Working the Program

As usual my code is available for checkout from the blog’s SNV repo:

svn checkout http://morethantechnical.googlecode.com/svn/trunk/ptam ptam

To get the stereo initialization you must press [spacebar] twice: Once when the camera has stabilized and the features are stable, and another time when the camera has translated and again stabilized.
This marks the 2 keyframes that will be used for stereo init and triangulation.
From that point on, the 3D scene will start and the track-and-estimate stage begins. Try not to move the camera violently as the optical flow may suffer.

Thanks Lior for your help getting the hang of these subjects, and the opportunity to meddle with a subject I long gone wanted to explore.

I hope everyone will enjoy and learn from my enjoyment and learning.

Bye!

Roy.