More Than Technical

Combining OpenCV 2.x and Microsoft WPF [w/ code]

Roy — Wed, 21 Jul 2010 21:30:59 +0000

Hi,
Just a quicky about OpenCV and Windows Presentation Framework interoperability. It’s really easy with a simple Managed C++ wrapper. What I’ll show is how to transfer an OpenCV cv::Mat into WPF’s BitmapSource. Let’s see how it’s done.

First some framework. I’ll define a managed C++ (ref) class that holds everything WPF needs to make a BitmapSource:

public ref class OpenCVImageWrapper {
	public:
		int width;
		int height;
		cli::array^ buf;
		int buf_size;
		int row_stride;
		int channels;
	};

That’s it, now let’s build a BitmapSource out of an instance of this class in C#:

OpenCVImageWrapper im = getFromOpenCV();
BitmapSource bs = BitmapSource.Create(
                im.width, im.height, 96, 96,
                System.Windows.Media.PixelFormats.Bgr24, //need to look out here: this is for BGR 24-bits images, you might need Gray8, Gray32Float or one of others...
                null, im.buf, im.row_stride);
imageOnScreen.Source = bs;

(imageOnScreen is an Image object I dragged and dropped into a window in the visual GUI builder)

Now we need a bit more managed C++ wizardry to fill up the wrapper, here it is:

OpenCVImageWrapper^ getFromOpenCV() {
Mat im = imread("someimage.jpg");

OpenCVImageWrapper^ i = gcnew OpenCVImageWrapper();

i->buf = gcnew cli::array(im.rows * im.step);
System::Runtime::InteropServices::Marshal::Copy(System::IntPtr::IntPtr(im.data),i->buf,0,im.rows * im.step);

i->width = im.cols;
i->height = im.rows;
i->row_stride = im.step;
i->channels = im.channels();
i->buf_size = im.rows * im.step;

return i;
}

We’re pretty much done. Basic stuff!

It’s not memory optimized as I’m copying the buffer, but it gets the job done.

Thanks
Roy.

Image Recoloring using Gaussian Mixture Model and Expectation Maximization [OpenCV, w/Code]

Roy — Thu, 24 Jun 2010 15:34:59 +0000

Hi,
I’ll present a quick and simple implementation of image recoloring, in fact more like color transfer between images, using OpenCV in C++ environment. The basis of the algorithm is learning the source color distribution with a GMM using EM, and then applying changes to the target color distribution. It’s fairly easy to implement with OpenCV, as all the “tools” are built in.

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(y);  //pointer to pixel data in the row
			uchar* mask_row = source_mask.ptr(y); //pointer to binary mask
			for(int x=0;x 0) {
					source_samples.at(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 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::max();
		vector best_res(num_g);
		vector prmt(num_g); 

		for(int itr = 0; itr < 10; itr++) {
			for(int i=0;i res(num_g);
			vector taken(num_g);
			for(int sg = 0; sg < num_g; sg++) {
				double min_dist = std::numeric_limits::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
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(y);
			uchar* mask_row = target_mask.ptr(y);
			for(int x=0;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
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.

Iterative Closest Point (ICP) for 2D curves with OpenCV [w/ code]

Roy — Sun, 06 Jun 2010 07:59:37 +0000

ICP – Iterative closest point, is a very trivial algorithm for matching object templates to noisy data. It’s also super easy to program, so it’s good material for a tutorial. The goal is to take a known set of points (usually defining a curve or object exterior) and register it, as good as possible, to a set of other points, usually a larger and noisy set in which we would like to find the object. The basic algorithm is described very briefly in wikipedia, but there are a ton of papers on the subject.

I’ll take you through the steps of programming it with OpenCV.

So the algorithm is basically

Find the points which are closest to our curve
Compute the best rotation and translation between our points and the closest points
Move our points according to the found transformation, and check the error
Run another iteration until convergence.

How easy is that?? OpenCV gives us the tools to do everything in there!

Finding closest points

In v2.x of OpCV they introduced the FLANN framework of range search methods. It’s fairly easy to use, and produces good results, although I did run into some problems with it before. Let’s see how it’s used:

float flann_knn(Mat& m_destinations, Mat& m_object, vector& ptpairs, vector& dists = vector()) {
	// find nearest neighbors using FLANN
	cv::Mat m_indices(m_object.rows, 1, CV_32S);
	cv::Mat m_dists(m_object.rows, 1, CV_32F);

	Mat dest_32f; m_destinations.convertTo(dest_32f,CV_32FC2);
	Mat obj_32f; m_object.convertTo(obj_32f,CV_32FC2);

	assert(dest_32f.type() == CV_32F);

	cv::flann::Index flann_index(dest_32f, cv::flann::KDTreeIndexParams(2));  // using 2 randomized kdtrees
    flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, cv::flann::SearchParams(64) ); 

    int* indices_ptr = m_indices.ptr(0);
    //float* dists_ptr = m_dists.ptr(0);
    for (int i=0;i
This code was practically ripped off OpenCV’s sample code, and worked straight up.. You can see I have 2 input matrices that define the “destination” points – these are the unknown, the “object” points – these are our points, and an output vector to mark the correspondence between the two sets. I also have a vector for distances between points of each pair.

I define a FLANN index object using KD-tree and perform a KNN (K nearest neighbors) search on it, for all the object points. After invoking the search function, it’s all garnish.
Compute transform
With the point pairs given, computing the transform between the two sets should be easy. And it is. I started by computing it using least-squares to find the parameters of the transformation: rotation angle, translation in X and Y directions. But my solution was not “resistant” to scale, so I looked up another easy solution to the problem, and surely enough a simple solution was found in this paper. 
void findBestReansformSVD(Mat& _m, Mat& _d) {
	Mat m; _m.convertTo(m,CV_32F);
	Mat d; _d.convertTo(d,CV_32F);

	Scalar d_bar = mean(d);
	Scalar m_bar = mean(m);
	Mat mc = m - m_bar;
	Mat dc = d - d_bar;

	mc = mc.reshape(1); dc = dc.reshape(1);

	Mat H(2,2,CV_32FC1);
	for(int i=0;i(0);
	Scalar T(d_bar[0] - (m_bar[0]*_R[0] + m_bar[1]*_R[1]),d_bar[1] - (m_bar[0]*_R[2] + m_bar[1]*_R[3]));

	m = m.reshape(1);
	m = m * R;
	m = m.reshape(2);
	m = m + T;// + m_bar;
	m.convertTo(_m,CV_32S);
}

I really just followed what they said in the paper: take the mean point off the two sets, build a correlation matrix from the distances, do SVD and use U and Vt for computing the rotation and translation. It actually works!
Putting it together
This is the easy part, just using these two functions and small while loop:
	while(true) {
		pair.clear(); dists.clear();
		double dist = flann_knn(destination, X, pair, dists);

		if(lastDist <= dist) {
			X = lastGood;
			break;	//converged?
		}
		lastDist = dist;
		X.copyTo(lastGood);

		cout << "distance: " << dist << endl;

		Mat X_bar(X.size(),X.type());
		for(int i=0;i(pair[i],0);
			X_bar.at(i,0) = p;
		}

		ShowQuery(destination,X,X_bar);

		X = X.reshape(2);
		X_bar = X_bar.reshape(2);
		findBestReansformSVD(X,X_bar);
		X = X.reshape(1); // back to 1-channel
	}

This will iteratively search for points and move the curve until the curve doesn’t move anymore.
Some proof

Code
Available in the SVN repo:
svn checkout https://morethantechnical.googlecode.com/svn/trunk/ICP ICP

Thanks for joining!

Roy.

Bust out your own graphcut based image segmentation with OpenCV [w/ code]

Roy — Wed, 05 May 2010 11:27:29 +0000

This is a tutorial on using Graph-Cuts and Gaussian-Mixture-Models for image segmentation with OpenCV in C++ environment.

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(i);
   for(int l=0;l
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> 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 areas(contours.size());
for(unsigned int ai=0;ai
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.

Congratulations! Roy is going to MIT

Arnon — Tue, 30 Mar 2010 10:10:55 +0000

I would like to congratulate my friend Roy, who got accepted to M.I.T in the Program in Media Arts and Sciences.
Starting this September, Roy will be spending the next two years in Boston.

I wish him all the best and luck.

I’m sure this degree will provide some interesting posts to this blog

Quick and Easy Head Pose Estimation with OpenCV [w/ code]

Roy — Fri, 19 Mar 2010 16:38:49 +0000

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:

Left ear
Right ear
Left eye
Right eye
Nose tip
Left mouth corner
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 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 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);

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

//display points on image
Mat img = imread("image.png");
for(unsigned int i=0;i();
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.

Implementing PTAM: stereo, tracking and pose estimation for AR with OpenCV [w/ code]

Roy — Sat, 06 Mar 2010 16:53:11 +0000

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 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 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& points1, vector& points2, vector& 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 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(oc);

	//scan all points, reproject 3D->2D, and keep only good ones
	for(int i=0;idata.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& points, vector& rv, vector& 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(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(3);
		tv[0]=0;tv[1]=0;tv[2]=0;
		tvec = Mat(tv);
	}

	//create a float rep  of points
	vector 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();
	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
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.

iPhone OS 3.x Raw data of camera frames

Roy — Sat, 27 Feb 2010 20:44:32 +0000

Hi All

It looks like it’s finally here – a way to grab the raw data of the camera frames on the iPhone OS 3.x.

Update: Apple officially supports this in iOS 4.x using AVFoundation, here’s sample code from Apple developer.

A gifted hacker named John DeWeese was nice enough to comment on a post from May 09′ with his method of hacking the APIs to get the frames. Though cumbersome, it looks like it should work, but I haven’t tried it yet. I promise to try it soon and share my results.

Way to go John!
Some code would be awesome…

Roy.

SmartHome – Embedded computing course project

Roy — Sun, 21 Feb 2010 16:42:09 +0000

Hi
In the past few weeks I have been working hard at a few projects for end-of-term at Uni. One of the projects is what I called “SmartHome”, for Embedded computing [link] course, is a home monitoring [link] application. In the course the students were given an LPC2148 arm7-MCU (NXP) based education board, implemented by Embedded Artists [link]. My partner Gil and I decided to work with ZigBee extension modules [link] to enable remote communication.

Here are the steps we took to bring this project to life.

The Idea

Our vision is to create a home monitoring and controlling system, that will enable tracking different sensors around the house and also control switches.The system should be centralized by a master controller, and also wireless so it will not need cumbersome wiring throughout the house. We would also like the system to be easily controlled by a PC, so visual information could be displayed to the user, as well as allow manual control of the electronic switches.

Such a system will be able to automatically:

Turn off the garden lighting when the light outside is bright enough,
Control the lawn watering system,
Control air conditioning in the house according to the temperature,
etc.

The Hardware

As I mentioned, we were given an LPC2148 education board [link], implemented by Embedded Artists, that boasts a 12Mhz ARM CPU, and many peripheral subsystems. Among the systems are: LCD screen, numerous LEDs, a LED matrix, a fan, analog dials, USB and UART I/O and more. The boards also has a port for connecting a ZigBee module to enable RF communication, but it doesn’t contain the actual module. Since we needed remote communication between our stations, we bought 3 XBee modules from Maxstream [link].

The Software

The LPC boards can be programmed very easily using tools provided by Embedded Artists, such as GCC with Newlib for compiling (both Win and Linux), and Flashmagic (for windows) or lpc21isp for loading the compiled program. We used these tools for programing the “embedded” part of the application, for the PC client we used Java with SWT and serial port connectivity (RxTx for Windows and Linux).

Embedded program

The embedded program on the LPC board has two parts: A master and A client. The system has only one master, and up to 3 clients. The master gathers information from the clients, and controls their behavior according to the user requests. For communication between the master and clients we created a communication protocol that has only a few simple messages:

INIT – The master sends this request to initialize the connection between itself and a client.
ACK – This is the response for every request.
POLL – The clients answers this request with the data from all it’s sensors.
OPERATE – The master commands the client to switch something on or off.
ERROR – A general error response.

All commands / responses have a unified structure described here:

General struct:
 ___________________________________________ _____
|    OP   | To | From | <----- DATA ------> | EOM
|_________|____|______|_____________________|_____

Data:

POLL (response)
_ ____________ ______ ______ ________
 | Temperature| ADC0 | ADC1 | Button |
_|____________|______|______|________|

OPERATE
_ _________
 | BITMASK |
_|_________|

This way we had a very easy implementation of the communication module, since we always had to look for only 14 bytes on the wire.

Communication with ZigBee module

To jumpstart our implementation we used the examples provided by Embedded Artists for operating the ZigBee module [link]. The communication with the ZigBee module is on the UART1 port of the MCU. The example code takes care of opening the correct GPIO pins, setting the IRQ masks to enable interrupts and provides a very simple API for transmitting and receiving characters with the XBee over UART (described in uart.h and uart.c files).

We took the XBee example code and expanded it to be able to recieve and send data between two stations. That means setting up each station’s module with it’s ID, address, channel and target address by AT commands [spec, see page. 28]: ATID, ATCH, ATMY and ATDL. Two other key features are putting the module into command mode (rather than transmit mode) to set the mentioned parameters, this is done by ‘+++’ to enter command mode and ATCN to exit. Once we had a decent framework to communicate between the stations, we started to build the logic.

The master station logic is like so:

Initialize XBee module.
INIT all client stations, and see which station answers – these will be our “up” stations.
Loop:
1. POLL each “up” station in a loop.
2. Take care of any OPERATE requests from the PC client.

This way, the client’s logic boils down to just looping, testing for any request and taking care of it. Both client and master share most of the code, so only the main process code is essentially different. The example code uses a framework called “Preemtive OS” to allow multitasking / processes (code was bundled in the examples).

Communication with PC

Communication between the master and PC client also required some lightweight “protocol”. The communication is again over UART (0 this time, 1 is used by XBee), only now the PC is doing a UART-over-USB with the board. The protocol we ended up with supports these features:

Commands the PC wants the master to perform look like “m=
=
”, such commands can be:
- “poll=”, send a POLL to the station indexed i

Notifications the master would like to share with the user (PC) look like “pc=”, such notifications can be:

“up=”, the station indexed i is up

“master_online”,

“temp= ”, the station indexed i has a temperature reading of temp

“e=”, error message from the master

This is an incomplete set of features, but it’s representative of the idea we want to present.

PC client program

The PC client we built using Java, so it would (and should) be portable between OSs. The GUI was built with the SWT, using the eclipse Visual Editor plug-in which simplified the process. Communication with the LPC board is done over the serial port of the board, as I mentioned the PC creates a virtual COM port using a USB-to-UART driver (FTDI, bundled with windows) [link].

To test the GUI we created a “simulator”, that mimics the operation of a master station, that is shown in the video above.

The high-level design of the program is described in this inheritance UML diagram:

Our PC client uses serial-port connectivity based on the old javax.serial APIs. Though these APIs have been abandoned by Sun, and there is no official Win32 implementation bundled with the JDK (only for *NIXs/Solaris), a project named RxTx is upkeeping an implementation of this API for windows [link].

The Code

We are releasing the code under the BSD license, for everyone to use, enjoy, learn and expand.

It is available via the blog’s SVN repo:

PC Client (requires RxTx serial port impl and SWT)

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

Embedded program (includes all dependencies)

svn checkout http://morethantechnical.googlecode.com/svn/trunk/smarthome_embedded/final project

To compile the master program “make” in the master directory, and you’ll get an “xbee_master.hex” file, same goes for the client in the client directory (these directories don’t contain code, only a makefile). Then you have to upload the hex into the board, this is done either by “make deploy” (if you have lpc21isp), or FlashMagic on Win.

Java is compiled as usual… just remember the dependencies.

We would like to thank Sivan Toledo for the guidance, loaned hardware and inspiration.

And thanks all for listening!
Roy S. & Gil Ramon

Stream your favorite radio station to your workplace

Arnon — Fri, 29 Jan 2010 14:53:00 +0000

I want to suggest a trick that worked for me. My work place blocks most of the popular radio stations stream sites in my country.
I can understand why they’re doing that, but hey – if you want to save bandwidth I suggest you block YouTube (not that I complain…)
Well, I thought of a way to listen to my favorite radio station from work, by re-streaming it from my home. And it worked!

It can also work for you, in case your IT does not block by protocol, only by address.

So here’s how to do it:

STEP 1 – Retrieve the actual link for your radio station

Go to the station’s website and try to get a link that you can play in Windows Media Center.
Some stations will let you find it, others won’t.
If this is the case, here’s what you need to do (At home!)

Download wireshark and install it

Start capturing using this filter: rtsp.request

Try to listen to the station

You should see such an output in wireshark:

As you can see, there is a url here that looks like this:
rtsp://someurl/somefile RTSP/1.0

Take this URL, and replace RTSP with MMS and omit the RTSP/1.0 ending.
Your URL should look like this: mms://someurl.somefile

Try to play this URK in windows media player. If it works – you’re half way through

If you can get the link in any other way, it’s fine… This is only a suggestion

STEP 2 – Set up VLC as a stream server at your home

I will start with an apology. I didn’t find a proper way to do it with the GUI of version 1.0+.
On the past versions it was relatively clear, so that’s where I managed to find the command line you need to run in order for it to work
So, at this point, you need to run VLC on your PC with these parameters:

vlc mms://yourserver/yourlink :sout=#transcode{acodec=mp3,ab=64,channels=2}:duplicate{dst=std{access=mmsh,mux=asfh,dst=:8080}}

This will stream the web-radio station to port 8080.
You can test this using Windows Media Player, just open it up, and open the url mms://localhost:8080

Another thing you need to keep in mind, is that you should establish port forwarding if you are behind NAT.

That’s the whole deal… Of course you can improve it by creating a script that will run it, or add the http interface of VLC to remote control it on another port, but I don’t want to overload this guide…

keep it simple!