The thing about triangulation is that you need to know the extrinsic parameters of your cameras – the difference in location and rotation between them.
To get the camera matrices… that’s a different story that I’m going to cover shortly in a tutorial (already in writing) about Structure from Motion.

But let’s assume that we already have the extrinsic matrices. In most cases, where you know what the motion is (you took the pictures , you can just write the matrices explicitly.

Linear Triangulation

/**
 From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997
 */
Mat_ LinearLSTriangulation(Point3d u,		//homogenous image point (u,v,1)
				   Matx34d P,		//camera 1 matrix
				   Point3d u1,		//homogenous image point in 2nd camera
				   Matx34d P1		//camera 2 matrix
								   )
{
	//build matrix A for homogenous equation system Ax = 0
	//assume X = (x,y,z,1), for Linear-LS method
	//which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
	Matx43d A(u.x*P(2,0)-P(0,0),	u.x*P(2,1)-P(0,1),		u.x*P(2,2)-P(0,2),
		  u.y*P(2,0)-P(1,0),	u.y*P(2,1)-P(1,1),		u.y*P(2,2)-P(1,2),
		  u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1),	u1.x*P1(2,2)-P1(0,2),
		  u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1),	u1.y*P1(2,2)-P1(1,2)
			  );
	Mat_ B = (Mat_(4,1) <<	-(u.x*P(2,3)	-P(0,3)),
					  -(u.y*P(2,3)	-P(1,3)),
					  -(u1.x*P1(2,3)	-P1(0,3)),
					  -(u1.y*P1(2,3)	-P1(1,3)));

	Mat_ X;
	solve(A,B,X,DECOMP_SVD);

	return X;
}

This method relies very simply on the principle that every 2D point in image plane coordinates is a projection of the real 3D point. So if you have two views, you can set up an overdetermined linear equation system to solve for the 3D position.

See how simple defining a Matx43d struct from scratch and using it in solve(..) is?
I tried doing some more fancy stuff with Mat.row(i) and Mat.col(i), trying to stick to Hartley’s description of the A matrix, but it just didn’t work.

Using it

Using this method is easy:

//Triagulate points
void TriangulatePoints(const vector& pt_set1,
					   const vector& pt_set2,
					   const Mat& Kinv,
					   const Matx34d& P,
					   const Matx34d& P1,
					   vector& pointcloud,
					   vector& correspImg1Pt)
{
#ifdef __SFM__DEBUG__
	vector depths;
#endif

	pointcloud.clear();
	correspImg1Pt.clear();

	cout << "Triangulating...";
	double t = getTickCount();
	unsigned int pts_size = pt_set1.size();
#pragma omp parallel for
	for (unsigned int i=0; i		Point2f kp = pt_set1[i];
		Point3d u(kp.x,kp.y,1.0);
		Mat_ um = Kinv * Mat_(u);
		u = um.at(0);
		Point2f kp1 = pt_set2[i];
		Point3d u1(kp1.x,kp1.y,1.0);
		Mat_ um1 = Kinv * Mat_(u1);
		u1 = um1.at(0);

		Mat_ X = IterativeLinearLSTriangulation(u,P,u1,P1);

//		if(X(2) > 6 || X(2) < 0) continue;

#pragma omp critical
		{
			pointcloud.push_back(Point3d(X(0),X(1),X(2)));
			correspImg1Pt.push_back(pt_set1[i]);
#ifdef __SFM__DEBUG__
			depths.push_back(X(2));
#endif
		}
	}
	t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "Done. ("<
	//show "range image"
#ifdef __SFM__DEBUG__
	{
		double minVal,maxVal;
		minMaxLoc(depths, &minVal, &maxVal);
		Mat tmp(240,320,CV_8UC3); //cvtColor(img_1_orig, tmp, CV_BGR2HSV);
		for (unsigned int i=0; i			double _d = MAX(MIN((pointcloud[i].z-minVal)/(maxVal-minVal),1.0),0.0);
			circle(tmp, correspImg1Pt[i], 1, Scalar(255 * (1.0-(_d)),255,255), CV_FILLED);
		}
		cvtColor(tmp, tmp, CV_HSV2BGR);
		imshow("out", tmp);
		waitKey(0);
		destroyWindow("out");
	}
#endif
}

Note that you must have the camera matrix K (a 3×3 matrix of the intrinsic parameters), or rather it’s inverse, noted here as Kinv.

Results and some discussion

3D view of the triangulated point cloud

Notice how stuff is distorted in the 3D view… but this is not due projective ambiguity! as I am using the Essential Matrix to obtain the camera P matrices (cameras are calibrated). Hartley and Zisserman explain this in their book on page 258, and the reasons for projective ambiguity (and how to resolve it) on page 265. The distortion must be due to inaccurate point correspondence…

The cool visualization is done using the excellent PCL library.

Iterative Linear Triangulation

Hartley, in his article “Triangulation” describes another triangulation algorithm, an iterative one, which he reports to “perform substantially better than the [...] non-iterative linear methods”. It is, again, very easy to implement, and here it is:

/**
 From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997
 */
Mat_<double> IterativeLinearLSTriangulation(Point3d u,	//homogenous image point (u,v,1)
											Matx34d P,			//camera 1 matrix
											Point3d u1,			//homogenous image point in 2nd camera
											Matx34d P1			//camera 2 matrix
											) {
	double wi = 1, wi1 = 1;
	Mat_<double> X(4,1);
	for (int i=0; i<10; i++) { //Hartley suggests 10 iterations at most
		Mat_<double> X_ = LinearLSTriangulation(u,P,u1,P1);
		X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X_(3) = 1.0;

		//recalculate weights
		double p2x = Mat_<double>(Mat_<double>(P).row(2)*X)(0);
		double p2x1 = Mat_<double>(Mat_<double>(P1).row(2)*X)(0);

		//breaking point
		if(fabsf(wi - p2x) <= EPSILON && fabsf(wi1 - p2x1) <= EPSILON) break;

		wi = p2x;
		wi1 = p2x1;

		//reweight equations and solve
		Matx43d A((u.x*P(2,0)-P(0,0))/wi,		(u.x*P(2,1)-P(0,1))/wi,			(u.x*P(2,2)-P(0,2))/wi,
				  (u.y*P(2,0)-P(1,0))/wi,		(u.y*P(2,1)-P(1,1))/wi,			(u.y*P(2,2)-P(1,2))/wi,
				  (u1.x*P1(2,0)-P1(0,0))/wi1,	(u1.x*P1(2,1)-P1(0,1))/wi1,		(u1.x*P1(2,2)-P1(0,2))/wi1,
				  (u1.y*P1(2,0)-P1(1,0))/wi1,	(u1.y*P1(2,1)-P1(1,1))/wi1,		(u1.y*P1(2,2)-P1(1,2))/wi1
				  );
		Mat_<double> B = (Mat_<double>(4,1) <<	-(u.x*P(2,3)	-P(0,3))/wi,
						  -(u.y*P(2,3)	-P(1,3))/wi,
						  -(u1.x*P1(2,3)	-P1(0,3))/wi1,
						  -(u1.y*P1(2,3)	-P1(1,3))/wi1
						  );

		solve(A,B,X_,DECOMP_SVD);
		X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X_(3) = 1.0;
	}
	return X;
}

(remember to define your EPSILON)
This time he works iteratively in order to minimize the reprojection error of the reconstructed point to the original image coordinate, by weighting the linear equation system.

Recap

So we’ve seen how easy it is to implement these triangulation methods using OpenCV’s nice Matx### and Mat_ structs.
Also solve(…,DECOMP_SVD) is very handy for overdetermined non-homogeneous linear equation systems.
Watch out for my Structure from Motion tutorial coming up, which will be all about using OpenCV to get point correspondence from pairs of images, obtaining camera matrices and recovering dense depth.

If you are looking for more robust solutions for SfM and 3D reconstructions, see:
http://phototour.cs.washington.edu/bundler/
http://code.google.com/p/libmv/
http://www.cs.washington.edu/homes/ccwu/vsfm/
Enjoy,
Roy.

Let’s start!

Some mathematical background

Don’t worry, it wont hurt. much.

So Spherical Harmonics, were invented to numerically express a whole bunch of things in physics like gravity and magnetic fields. But they also became very useful for computer graphics as they are perfect for modelling light falling on a spherical body.

But what ARE those mysterious spherical harmonics?

The way I see it, they are a series of “modes” or “eigenvectors” or “orthogonal components” of a base that spans the surface of a sphere.
To put it simple, they describe the surface of a sphere in increasing finer grained portions. Much like a Fourier decomposition does to a function, there is the base and there are coefficients that when multiplied with the base they recover the function.

How is that good for graphics?

People have used spherical harmonics mostly to model lighting of spherical objects. When you know the coefficients that describe the lighting, you can change them to Re-light an object, or De-light, or transfer the lighting conditions of one scene to another. Very useful!

Some good researchers, Basri and Jacobs, back in 2001 have formulated the first 9 harmonics as a function of the surface normal. On this page Basri references all his work on the subject: http://www.wisdom.weizmann.ac.il/~ronen/index_files/harmonic.html

But I like to reference a work that’s easier to process than Basri’s, that is the work of Wang et al from 2007. These guys made the steps to use spherical harmonics easier to follow: http://research.microsoft.com/en-us/um/people/zliu/cvpr2007.pdf.
But their algorithm is quite advanced, as it solves not only for the harmonics’ coefficients but also for the normals of the object in the image. They use some fancy optimization of an energy function over a graph, that I’m not going to discuss.
But they did make the process of finding the spherical harmonics’ coefficient very clear.

The bottom line

We should solve for a vector of 9 coefficients that describes the “lighting of the object” (a face in our case).
Each coefficient will tell us how much that specific harmonic is strong or weak, or in other words how lit is that certain area of the object.

Wang and Basri show a very simple method of using simultaneous linear equations to solve for the lighting coefficients, it depends only on knowing the normal of the object’s surface at each pixel in the image.

Getting the normals of a canonical face

So to get the normals, I thought the best way is to use a canonical model of a face (some king of an average face), instead of trying to recover the normals from the image pixels.
For that end, I used Rhino3D to model (very roughly) a shape that resembles a human face, starting from an elongated sphere.
Now all that’s left is to align the model with the face to relight, and that will supply the normals.

Cool. Then I built a small app that allows the user to move the model around until it’s aligned with the face image. I used FLTK 3.0 to do it since they have a simple interface with OpenGL, they are cross platform, and lightweight.
So I set up a scene where I have the image as the background, and the model is floating above it, half transparent so the user can find the right spot. I added functions for rotating the model, and extra stuff like turning the model opaque.

To get the normal map I used a very simple GLSL shader, that simply colors the pixel with the value of the normal nX,nY,nZ -> R,G,B.
This way the result image OpenGL renders is simply the normal map of the face model. I just grab it using glReadPixels.

Estimating spherical harmonics

So, after the model is aligned, we can assume we have the normals ready for us for each pixel in the image, and the intensity in each pixel is also known.
The first step that Wang suggests, without knowledge of the real face albedo (the real color of every pixel without any lighting effect), is to get an approximation of the 9-vector of lighting coefficients by setting a constant albedo. Easy enough, we can set the albedo to the average color in the face.
Then we can simply build a huge set of linear equations (huge as the number of pixels in the image), and solve an overdetermined system to get the 9 coefficients.

		Scalar albedo_constant = mean(face_img_hsv, smallFaceMask);

		//setup linear equation system, lighting coefficients (l) is unknown
		//I = p00 * Ht * l
		float p00 = (float)albedo_constant[2] / 255.0f;

		cout << "Build Ht("<<n<<",9)...";
		cout << "Build I("<<n<<",1)...";
		//build Ht and I
		Mat_<float> Ht(n,9);
		Mat_<float> I(n,1);
		int pos = 0;
		vector<Mat_<uchar> > face_img_chnls; split(face_img_hsv, face_img_chnls);
		for (int i=0; i<normalMapFlat.rows; i++) {
			if (smallFaceMask(i) == 0) { //is this pixel on the face?
				continue;
			}
			Ht.row(pos) = p00 * calculateSphericalHarmonicsForNormal(normalMapFlat(i));
			I(pos,0) = face_img_chnls[2](i) / 255.0f; //get V from HSV of pixel [0,1]
			pos ++;
		}
		cout << "DONE"  << endl;

		cout << "Solve" <<endl;
		solve(Ht, I, l, DECOMP_SVD);

		cout << "initial lighting coeffs: ";
		for (int i=0; i<l.rows; i++) {
			cout<<l.at<float>(i)<<",";
		}

Booyah! lighting coefficients.

But this is only the first step. Now we can get an approximation of the albedo as well, using the coefficients:

		Mat_<Vec3b> face_img_v3b = face_img;

		#pragma omp parallel for schedule(dynamic)
		for (int y=0; y<face_img.rows; y++) {
			for (int x=0; x<face_img.cols; x++) {
				if (face_mask(y,x) == 0) {
					albedo(y,x) = 0;
					continue;
				}
				Mat sph = calculateSphericalHarmonicsForNormal(normalMap(y,x));
				Mat_<float> sph_l = sph * l;
				float fsph_l = sph_l(0);

				for (int cn = 0; cn<3; cn++) {
					float fimg = face_img_v3b(y,x)[cn] / 255.0f;
					albedo(y,x)[cn] = (fimg / fsph_l);
				}
			}
		}

Done.
Now that we have an initial albedo, Wang suggests we compute the coefficients again to get a better approximation, and then the albedo again.
I however ran into some problems trying to do the second iteration, and the results always came out too dark… But even with the first iteration you can see a very nice change.
Look at the video from before, you can see the right side of the face, which is over-lit, was darkened and the left side was lit up.

Code

The code for spherical harmonics analysis of images is part of a bigger project I have been working on for some time. I also spoke of it in a previous post.
Anyway it’s up in GitHub: https://github.com/royshil/HeadReplacement/tree/master/HeadReplacement
You’re looking for 4 files:

• SpharmonicsUI.cpp
• SpharmonicsUI.h
• spherical_harmonics_analysis.cpp
• spherical_harmonics_analysis.h

You can use the CMakeLists.txt to compile, but here’s a CMakeLists.txt that should take you there in one piece (fingers crossed):

find_package(OpenCV REQUIRED)
find_package(OpenGL REQUIRED)
find_package(OpenMP REQUIRED)

######## Find and add GLEE ########
file(GLOB_RECURSE GLEE_PATH "${CMAKE_SOURCE_DIR}/GLee.c")
if(GLEE_PATH STREQUAL GLEE_PATH-NOTFOUND)
	message(STATUS "GLEE was not found")
else()
	list(LENGTH GLEE_PATH GLEE_PATH_LEN)
	if(GLEE_PATH_LEN GREATER 1)
		list(GET GLEE_PATH 1 GLEE_PATH)
	endif()
	file(RELATIVE_PATH GLEE_PATH ${CMAKE_SOURCE_DIR} ${GLEE_PATH})
	get_filename_component(GLEE_PATH ${GLEE_PATH} REALPATH)
	get_filename_component(GLEE_PATH ${GLEE_PATH} PATH)
	message(STATUS "Found GLEE at ${GLEE_PATH}")
	add_library(GLEE ${GLEE_PATH}/GLee.c)
endif()

############ Find FLTK ############
if(NOT DEFINED FLTK_PATH)
	file(GLOB_RECURSE FLTK_PATH "${CMAKE_SOURCE_DIR}/Widget.h")
	if(FLTK_PATH STREQUAL FLTK_PATH-NOTFOUND   OR   FLTK_PATH STREQUAL "")
		message(STATUS "FLTK was not found !!!!!")
	else()
		list(LENGTH FLTK_PATH FLTK_PATH_LEN)
		if(FLTK_PATH_LEN GREATER 1)
			list(GET FLTK_PATH 1 FLTK_PATH)
		endif()
		file(RELATIVE_PATH FLTK_PATH ${CMAKE_SOURCE_DIR} ${FLTK_PATH})
		get_filename_component(FLTK_PATH ${FLTK_PATH} REALPATH)
		get_filename_component(FLTK_PATH ${FLTK_PATH} PATH)
		message(STATUS "Found FLTK at ${FLTK_PATH}")
	endif()
else()
	get_filename_component(FLTK_PATH ${FLTK_PATH} REALPATH)
	message(STATUS "FLTK path set to ${FLTK_PATH}")
endif()
set(FLTK_INCLUDE_DIR ${FLTK_PATH}/include)
set(FLTK_LIB_DIR ${FLTK_PATH}/lib)

######## Relighting #######
include_directories(${FLTK_INCLUDE_DIR})
include_directories(${OpenGL_INCLUDE_DIRS})
include_directories(${GLEE_PATH})
add_library(VirtualSurgeon_Relighting
	../HeadReplacement/glm.cpp
	../HeadReplacement/spherical_harmonics_analysis.cpp
	../HeadReplacement/LaplacianBlending.cpp
	../HeadReplacement/SpharmonicsUI.cpp
	../HeadReplacement/OGL_OCV_common.cpp
	)

Note that I had to resort to some very dark magic to recover the location of FLTK and GLEE… But it’s a jungle out there.

The source of the photograph is: http://www.flickr.com/photos/roel1943/309048020/
It is released under Creative Commons 2.0 ShareAlike-Attribution. So all the results here are also CC-2.0-SA-A…

Enjoy,
Roy.

I would like to present something I have been working on recently, a work that immensely affect what I wrote in the blog in the past two years…

To use it:
Go on this page,
Watch the short instruction video,
download the application (MacOSX-Intel-x64 Win32)
and make yourself a model!
It takes just a couple of minutes and it’s very simple…

This work is an academic research project, Please please, take the time to fill out the survey! It is very short..
The results of the survey (the survey alone, no photos of your work) will possibly be published in an academic paper.

Note: No information is sent anywhere in any way outside of your machine (you may even unplug the network). All results are saved locally on your computer, and no inputs are recorded or transmitted. The application contains no malware. The source is available here.

Note II: All stock photos of models used in the application are released under Creative Commons By-NC-SA 2.0 license. Creator: http://www.flickr.com/photos/kk/. If you wish to distribute your results, they should also be released under a CC-By-NC-SA 2.0 license.

Thank you!
Roy.

#include "opencv2/opencv.hpp"

using namespace cv;

class LaplacianBlending {
private:
	Mat_<Vec3f> left;
	Mat_<Vec3f> right;
	Mat_<float> blendMask;

	vector<Mat_<Vec3f> > leftLapPyr,rightLapPyr,resultLapPyr;
	Mat leftSmallestLevel, rightSmallestLevel, resultSmallestLevel;
	vector<Mat_<Vec3f> > maskGaussianPyramid; //masks are 3-channels for easier multiplication with RGB

	int levels;

	void buildPyramids() {
		buildLaplacianPyramid(left,leftLapPyr,leftSmallestLevel);
		buildLaplacianPyramid(right,rightLapPyr,rightSmallestLevel);
		buildGaussianPyramid();
	}

	void buildGaussianPyramid() {
		assert(leftLapPyr.size()>0);

		maskGaussianPyramid.clear();
		Mat currentImg;
		cvtColor(blendMask, currentImg, CV_GRAY2BGR);
		maskGaussianPyramid.push_back(currentImg); //highest level

		currentImg = blendMask;
		for (int l=1; l<levels+1; l++) {
			Mat _down;
			if (leftLapPyr.size() > l) {
				pyrDown(currentImg, _down, leftLapPyr[l].size());
			} else {
				pyrDown(currentImg, _down, leftSmallestLevel.size()); //smallest level
			}

			Mat down;
			cvtColor(_down, down, CV_GRAY2BGR);
			maskGaussianPyramid.push_back(down);
			currentImg = _down;
		}
	}

	void buildLaplacianPyramid(const Mat& img, vector<Mat_<Vec3f> >& lapPyr, Mat& smallestLevel) {
		lapPyr.clear();
		Mat currentImg = img;
		for (int l=0; l<levels; l++) {
			Mat down,up;
			pyrDown(currentImg, down);
			pyrUp(down, up, currentImg.size());
			Mat lap = currentImg - up;
			lapPyr.push_back(lap);
			currentImg = down;
		}
		currentImg.copyTo(smallestLevel);
	}

	Mat_<Vec3f> reconstructImgFromLapPyramid() {
		Mat currentImg = resultSmallestLevel;
		for (int l=levels-1; l>=0; l--) {
			Mat up;

			pyrUp(currentImg, up, resultLapPyr[l].size());
			currentImg = up + resultLapPyr[l];
		}
		return currentImg;
	}

	void blendLapPyrs() {
		resultSmallestLevel = leftSmallestLevel.mul(maskGaussianPyramid.back()) +
									rightSmallestLevel.mul(Scalar(1.0,1.0,1.0) - maskGaussianPyramid.back());
		for (int l=0; l<levels; l++) {
			Mat A = leftLapPyr[l].mul(maskGaussianPyramid[l]);
			Mat antiMask = Scalar(1.0,1.0,1.0) - maskGaussianPyramid[l];
			Mat B = rightLapPyr[l].mul(antiMask);
			Mat_<Vec3f> blendedLevel = A + B;

			resultLapPyr.push_back(blendedLevel);
		}
	}

public:
	LaplacianBlending(const Mat_<Vec3f>& _left, const Mat_<Vec3f>& _right, const Mat_<float>& _blendMask, int _levels):
	left(_left),right(_right),blendMask(_blendMask),levels(_levels)
	{
		assert(_left.size() == _right.size());
		assert(_left.size() == _blendMask.size());
		buildPyramids();
		blendLapPyrs();
	};

	Mat_<Vec3f> blend() {
		return reconstructImgFromLapPyramid();
	}
};

Mat_<Vec3f> LaplacianBlend(const Mat_<Vec3f>& l, const Mat_<Vec3f>& r, const Mat_<float>& m) {
	LaplacianBlending lb(l,r,m,4);
	return lb.blend();
}

To use, simply call the function LaplacianBlend with your two images and your mask, and the result will be returned.
Here’s something I did with it:

Enjoy
Roy.

OK let’s get down to business.

I followed a very simple technique described in this paper. I know, you say, “A Paper? Really? I’m not gonna read that technical boring stuff, give the bottom line! man.. geez.” Well, you are right, except that this paper IS the bottom line, it’s dead simple. It’s almost a tutorial. It is also referenced by the OpenCV documentation.

The method is simple:
- Extract features of choice from training set that contains all classes.
- Create a vocabulary of features by clustering the features (kNN, etc). Let’s say 1000 features long.
- Train your classifiers (SVMs, Naive-Bayes, boosting, etc) on training set again (preferably a different one), this time check the features in the image for their closest clusters in the vocabulary. Create a histogram of responses for each image to words in the vocabulary, it will be a 1000-entries long vector. Create a sample-label dataset for the training.
- When you get an image you havn’t seen – run the classifier and it should, god willing, give you the right class.

Turns out, those crafty guys in WillowGarage have done pretty much all the heavy lifting, so it’s up for us to pick the fruit of their hard work. OpenCV 2.3 comes packed with a set of classes, whose names start with BOW for Bag Of Words, that help a lot with implementing this method.

Starting with the first step:

Mat training_descriptors(1,extractor->descriptorSize(),extractor->descriptorType());

SurfFeatureDetector detector(400);
vector keypoints;

// computing descriptors
Ptr extractor(
   new OpponentColorDescriptorExtractor(
      Ptr(new SurfDescriptorExtractor())
   )
);

while(..loop a directory? a file?..) {
   Mat img = imread(filepath);
   detector.detect(img, keypoints);
   extractor->compute(img, keypoints, descriptors);
   training_descriptors.push_back(descriptors);
}

Simple!
Let’s go create a vocabulary then. Luckily, OpenCV has taken care of that, and provide a simple API:

BOWKMeansTrainer bowtrainer(1000); //num clusters
bowtrainer.add(training_descriptors);
Mat vocabulary = bowtrainer.cluster();

Boom. Vocabulary.
Now, let’s train us some SVM classifiers!
We’re gonna train a 2-class SVM, in a 1-vs-all kind of way. Meaning we train an SVM that can say “yes” or “no” when choosing between one class and the rest of the classes, hence 1-vs-all.
But first, we need to scour the training set for our histograms (the responses to the vocabulary, remember?):

vector<KeyPoint> keypoints;
Mat response_hist;
Mat img;
string filepath;
map<string,Mat> classes_training_data;

Ptr<FeatureDetector > detector(new SurfFeatureDetector());
Ptr<DescriptorMatcher > matcher(new BruteForceMatcher<L2<float> >());
Ptr<DescriptorExtractor > extractor(new OpponentColorDescriptorExtractor(Ptr<DescriptorExtractor>(new SurfDescriptorExtractor())));
Ptr<BOWImgDescriptorExtractor> bowide(new BOWImgDescriptorExtractor(extractor,matcher));
bowide->setVocabulary(vocabulary);

#pragma omp parallel for schedule(dynamic,3)
for(..loop a directory?..) {
   img = imread(filepath);
   detector->detect(img,keypoints);
   bowide.compute(img, keypoints, response_hist);

   #pragma omp critical
   {
      if(classes_training_data.count(class_) == 0) { //not yet created...
         classes_training_data[class_].create(0,response_hist.cols,response_hist.type());
         classes_names.push_back(class_);
      }
      classes_training_data[class_].push_back(response_hist);
   }
   total_samples++;
}

Now, two things:
First notice I’m keeping the training data for each class separately, this is because we will need this for later creating the 1-vs-all samples-labels matrices.
Second, I use OpenMP multi(-threading)processing to make the calculation parallel, and hence faster, on multi-core machines (like the one I used). Time is sliced by a whole lot. OpenMP is a gem, use it more. Just a couple of #pragma directives and you’re multi-threading.

Alright, data gotten, let’s get training:

#pragma omp parallel for schedule(dynamic)
for (int i=0;i<classes_names.size();i++) {
   string class_ = classes_names[i];
   cout << omp_get_thread_num() << " training class: " << class_ << ".." << endl;

   Mat samples(0,response_cols,response_type);
   Mat labels(0,1,CV_32FC1);

   //copy class samples and label
   cout << "adding " << classes_training_data[class_].rows << " positive" << endl;
   samples.push_back(classes_training_data[class_]);
   Mat class_label = Mat::ones(classes_training_data[class_].rows, 1, CV_32FC1);
   labels.push_back(class_label);

   //copy rest samples and label
   for (map<string,Mat>::iterator it1 = classes_training_data.begin(); it1 != classes_training_data.end(); ++it1) {
      string not_class_ = (*it1).first;
      if(not_class_.compare(class_)==0) continue; //skip class itself
      samples.push_back(classes_training_data[not_class_]);
      class_label = Mat::zeros(classes_training_data[not_class_].rows, 1, CV_32FC1);
      labels.push_back(class_label);
   }

   cout << "Train.." << endl;
   Mat samples_32f; samples.convertTo(samples_32f, CV_32F);
   if(samples.rows == 0) continue; //phantom class?!
   CvSVM classifier;
   classifier.train(samples_32f,labels);

   //do something with the classifier, like saving it to file
}

Again, I parallelize, although the process is not too slow.
Note how I build the samples and the labels, where each time I put in the positive samples and mark the labels ’1′, and then I put the rest of the samples and label them ’0′.

Moving on to …. testing the classifiers!
Nothing seems to me like more fun than creating a confusion matrix! Not really, but let’s see how it’s done:

map<string,map<string,int> > confusion_matrix; // confusionMatrix[classA][classB] = number_of_times_A_voted_for_B;
map<string,CvSVM> classes_classifiers; //This we created earlier

vector<string> files; //load up with images
vector<string> classes; //load up with the respective classes

for(..loop over a directory?..) {
   Mat img = imread(files[i]),resposne_hist;

   vector<KeyPoint> keypoints;
   detector->detect(img,keypoints);
   bowide->compute(img, keypoints, response_hist);

   float minf = FLT_MAX; string minclass;
   for (map<string,CvSVM>::iterator it = classes_classifiers.begin(); it != classes_classifiers.end(); ++it) {
      float res = (*it).second.predict(response_hist,true);
      if (res < minf) {
         minf = res;
         minclass = (*it).first;
      }
   }
   confusion_matrix[minclass][classes[i]]++;
}

When you take a look in my files, you will find a much complicated way of doing this. But this is the core idea – look in the image for the response histogram to the vocabulary of features (rather, feature-cluster-ceneters), run it by all the classifiers and take the one with the best score. Simple.
Consider making this parallel as well. No reason for it to be serial.

That’s about covers it.

Code

Lately I’m pushing stuff in Github.com using git rather than SVN on googlecode. Donno why, it’s just like that.
Get the whole thing at:
https://github.com/royshil/FoodcamClassifier

Follow the build instructions, they’re a breeze, and then follow the runnning instructions. It’s basically a series of command-line programs you run to get through each step, and in the end you have like a “predictor” service that takes an image and produces a prediction.

OK guys, have fun classifying stuff!
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.

Start from a simple Kinect frames feed from here:

int main(int argc, char **argv) {
	bool die(false);

	Mat depthMat(Size(640,480),CV_16UC1);
	Mat depthf  (Size(640,480),CV_8UC1);
	Mat rgbMat(Size(640,480),CV_8UC3,Scalar(0));
	Mat ownMat(Size(640,480),CV_8UC3,Scalar(0));

        Freenect::Freenect freenect;
        MyFreenectDevice& device = freenect.createDevice<MyFreenectDevice>(0);

	device.startVideo();
	device.startDepth();

    while (!die) {
    	device.getVideo(rgbMat);
    	device.getDepth(depthMat);
    	depthMat.convertTo(depthf, CV_8UC1, 255.0/2048.0);
        cv::imshow("depth",depthf);
		char k = cvWaitKey(5);
		if( k == 27 ){
			break;
		}
    }

   	device.stopVideo();
	device.stopDepth();
	return 0;
}

Now let’s stretch the signal a little bit and add the inpainting:

		//interpolation & inpainting
		{
			Mat _tmp,_tmp1; //minimum observed value is ~440. so shift a bit
			Mat(depthMat - 400.0).convertTo(_tmp1,CV_64FC1);

			Point minLoc; double minval,maxval;
			minMaxLoc(_tmp1, &minval, &maxval, NULL, NULL);
			_tmp1.convertTo(depthf, CV_8UC1, 255.0/maxval);  //linear interpolation

                       //use a smaller version of the image
			Mat small_depthf; resize(depthf,small_depthf,Size(),0.2,0.2);
                        //inpaint only the "unknown" pixels
			cv::inpaint(small_depthf,(small_depthf == 255),_tmp1,5.0,INPAINT_TELEA);

			resize(_tmp1, _tmp, depthf.size());
			_tmp.copyTo(depthf, (depthf == 255));  //add the original signal back over the inpaint
		}

Note that I’m using a small copy of the image, because inpainting is a heavy computation, and it works best on low frequencies. I copy back the original signal over the up-sized inpainted one to retain high frequencies.

It works pretty well!

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.

Another quicky on how to use Kinect (libfreenect) with OpenCV 2.1. I already saw people do it, but havn’t seen code.

UPDATE (12/29): OpenKinect posted very good C++ code of using libfreenect with OpenCV2.X APIs: here it is. Plus, their git repo now has a very clean C code: here it is.

So here it goes

Before I started, I got libfreenect off the OpenKinect git repo: https://github.com/OpenKinect/libfreenect
Which comes bundeled with “glview.c”, an example of how to use it with OpenGL, that I take some stuff from.
So get past the hurdle of compiling, and if glview runs – you’re home free. Play around and familiarize yourself with the code, this is going to be fast…

The only tricky thing is that OpenCV wants the “imshow”s to be on the main thread. This I learned empirically, form trial and error. But, in the glview.c example they do the “freenect_process_events” on the main thread, and the rendering is done on the GL thread. I flipped things around a bit.

So in the main function I have the initialization stuff and a pthread_create to make a freenect thread:

Mat depthMat(Size(640,480),CV_16UC1),
	rgbMat(Size(640,480),CV_8UC3,Scalar(0));
pthread_t fnkt_thread;
freenect_device *f_dev;
pthread_mutex_t buf_mutex = PTHREAD_MUTEX_INITIALIZER;
freenect_context *f_ctx;
pthread_cond_t frame_cond = PTHREAD_COND_INITIALIZER;

int main(int argc, char **argv)
{
	int res;

	g_argc = argc;
	g_argv = argv;

	if (freenect_init(&f_ctx, NULL) < 0) {
		printf("freenect_init() failed\n");
		return 1;
	}

	freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);

	int nr_devices = freenect_num_devices (f_ctx);
	printf ("Number of devices found: %d\n", nr_devices);

	int user_device_number = 0;
	if (argc > 1)
		user_device_number = atoi(argv[1]);

	if (nr_devices < 1)
		return 1;

	if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
		printf("Could not open device\n");
		return 1;
	}

	freenect_set_tilt_degs(f_dev,freenect_angle);
	freenect_set_led(f_dev,LED_RED);
	freenect_set_depth_callback(f_dev, depth_cb);
	freenect_set_rgb_callback(f_dev, rgb_cb);
	freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
	freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);

	freenect_start_depth(f_dev);
	freenect_start_rgb(f_dev);

	res = pthread_create(&fnkt_thread, NULL, freenect_threadfunc, NULL);
	if (res) {
		printf("pthread_create failed\n");
		return 1;
	}

	while (!die) {
		fr++;

		imshow("rgb", rgbMat);
		depthMat.convertTo(depthf, CV_8UC1, 255.0/2048.0);
		imshow("depth",depthf);			

                 char k = cvWaitKey(5);
                 if( k == 27 ) break;
         }

	printf("-- done!\n");

	destroyWindow("rgb");
	destroyWindow("depth");

	pthread_join(fnkt_thread, NULL);
	pthread_exit(NULL);
}

The freenect thread is simply

void *freenect_threadfunc(void* arg) {
	cout << "freenect thread"<<endl;
	while(!die && freenect_process_events(f_ctx) >= 0 ) {}
	cout << "freenect die"<<endl;
	return NULL;
}

And the two callbacks need also to write into the OpenCV buffers:

void depth_cb(freenect_device *dev, freenect_depth *depth, uint32_t timestamp)
{
	pthread_mutex_lock(&buf_mutex);

	//copy to ocv buf...
	memcpy(depthMat.data, depth, FREENECT_DEPTH_SIZE);

	got_frames++;
	pthread_cond_signal(&frame_cond);
	pthread_mutex_unlock(&buf_mutex);
}

void rgb_cb(freenect_device *dev, freenect_pixel *rgb, uint32_t timestamp)
{
	pthread_mutex_lock(&buf_mutex);
	got_frames++;
	//copy to ocv_buf..
	memcpy(rgbMat.data, rgb, FREENECT_RGB_SIZE);

	pthread_cond_signal(&frame_cond);
	pthread_mutex_unlock(&buf_mutex);
}

And that’s all you’ll need.

Now go crazy with your concoction of computer vision algorithms!

Roy.

Just wanted to share a bit of code using OpenCV’s camera extrinsic parameters recovery, camera position and rotation – solvePnP (or it’s C counterpart cvFindExtrinsicCameraParams2). I wanted to get a simple planar object surface recovery for augmented reality, but without using any of the AR libraries, rather dig into some OpenCV and OpenGL code.
This can serve as a primer, or tutorial on how to use OpenCV with OpenGL for AR.

The program is just a straightforward optical flow based tracking, fed manually with four points which are the planar object’s corners, and solving camera-pose every frame. Plain vanilla AR.

Well the whole cpp file is ~350 lines, but there will only be 20 or less interesting lines… Actually much less. Let’s see what’s up

I wanna run you through the code really quickly and not go into much detail, to keep thing simple. So first of all, we should have two separate threads: Vision and Graphics. The vision thread will track and solve, and the graphics thread will display.

Initialize

int main(int argc, char** argv) {
	initGL(argc,argv);
	initOCV(NULL);

	pthread_t tId;
	pthread_attr_t tAttr;
	pthread_attr_init(&tAttr);
	pthread_create(&tId, &tAttr, startOCV, NULL);

	startGL(NULL);
}

The initGL, initOCV functions just initialize stuff that can’t be initialized statically, like GLUT window definitions, some starting values for the cam-pose estimation and other boring stuff.

GLUT will run off the main thread, it seems putting it on its own thread makes it unhappy and not work.

Tracking

I’m using the simplest form of optical flow in OpenCV (LK Pyramid), and the code is equally very minimal..

void* startOCV(void* arg) {
	while (1) {
		cvtColor(img, prev, CV_BGR2GRAY);

		//get frame off camera
		cap >> frame;
		if(frame.data == NULL) break;

		frame.copyTo(img);

		cvtColor(img, next, CV_BGR2GRAY);

		//calc optical flow
		calcOpticalFlowPyrLK(prev, next, points1, points2, status, err, Size(30,30));
		cvtPtoKpts(imgPointsOnPlane, points2);

		//switch points vectors (next becomes previous)
		points1.clear();
		points1 = points2;

		//calculate camera pose
		getPlanarSurface(points1);

		//refresh 3D scene
		glutPostWindowRedisplay(glutwin);

		//show tracked points on scene
		drawKeypoints(next, imgPointsOnPlane, img_to_show, Scalar(255));
		imshow("main2", img_to_show);
		int c = waitKey(30);
		if (c == ' ') {
			waitKey(0);
		}
	}
	return NULL;
}

To use OpenCV’s ‘drawKeypoints’, which makes drawing key points much easier, we must use vector<KeyPoint>. So I created these 2 very simple converter funcs: cvtKeyPtoP and cvtPtoKpts.

You think ‘getPlanarSurface’ is complicated? think again! 3 lines:

void getPlanarSurface(vector<Point2f>& imgP) {
	Rodrigues(rotM,rvec);

	solvePnP(objPM, Mat(imgP), camera_matrix, distortion_coefficients, rvec, tvec, true);

	Rodrigues(rvec,rotM);
}

Booya! Vision stuff is done.

3D Graphics

A little 3D never hurt any AR system… But drawing it is very simple still:

void display(void)
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	//Make sure we have a background image buffer
	if(img_to_show.data != NULL) {
		Mat tmp; 

		//Switch to Ortho for drawing background
		glMatrixMode(GL_PROJECTION);
		glPushMatrix();
		gluOrtho2D(0.0, 0.0, 640.0, 480.0);

		glMatrixMode(GL_MODELVIEW);

		//Textures can only have power-of-two dimensions, so closest to 640x480 is 1024x512
		tmp = Mat(Size(1024,512),CV_8UC3);
		//However we are going to use only a portion, so create an ROI
		Mat ttmp = tmp(Range(0,img_to_show.rows),Range(0,img_to_show.cols));

		//Some frames could be 8bit grayscale, so make sure on the output we always get 24bit RGB.
		if(img_to_show.step == img_to_show.cols)
			cvtColor(img_to_show, ttmp, CV_GRAY2RGB);
		else if(img_to_show.step == img_to_show.cols * 3)
			cvtColor(img_to_show, ttmp, CV_BGR2RGB);
		flip(ttmp,ttmp,0);

		glEnable(GL_TEXTURE_2D);
		glTexImage2D(GL_TEXTURE_2D, 0, 3, 1024, 512, 0, GL_RGB, GL_UNSIGNED_BYTE, tmp.data);

		//Finally, draw the texture using a simple quad with texture coords in corners.
		glPushMatrix();
		glTranslated(-320.0, -240.0, -500.0);//why these parameters?!
		glBegin(GL_QUADS);
		glTexCoord2i(0, 0); glVertex2i(0, 0);
		glTexCoord2i(1, 0); glVertex2i(640, 0);
		glTexCoord2i(1, 1); glVertex2i(640, 480);
		glTexCoord2i(0, 1); glVertex2i(0, 480);
		glEnd();
		glPopMatrix();

		glMatrixMode(GL_PROJECTION);
		glPopMatrix();
		glMatrixMode(GL_MODELVIEW);
	}

	glPushMatrix();
	double m[16] = {	_d[0],-_d[3],-_d[6],0,
						_d[1],-_d[4],-_d[7],0,
						_d[2],-_d[5],-_d[8],0,
						tv[0],-tv[1],-tv[2],1	};

	//Rotate and translate according to result from solvePnP
	glLoadMatrixd(m);

	//Draw a basic cube
	glDisable(GL_TEXTURE_2D);
	glColor3b(255, 0, 0);
	glutSolidCube(1);
	glPopMatrix();

	glutSwapBuffers();
}

Not so horrific, huh? Most of it is drawing the background texture, and that’s only trying to avoid using glDrawPixels… The only interesting thing is loading the rotation and translation matrix.
However you will notice the tv[0] (x axis component of translation) doesn’t have a minus sign, that’s because OpenCV’s solvePnP assumes looking down the -z axis, while OpenGL assumes looking up the +z axis (so a 180 rotation around the x axis is needed). Same goes for _d[0] _d[1] and _d[2].
OpenGL in fact is defaulting to the camera looking down the -y axis, where the z axis is facing up (z is elevation). But in initGL I initialized OpenGL to look “normally” down the -z axis where +x goes right and +y goes up.

Proof time

Not that you need it.. But here’s a video of it working.

BTW: If anyone can solve the problem of the slight misalignment of the 3D and image – let me know.

Code and Salutations

Code can be downloaded from blog’s SVN:

svn checkout http://morethantechnical.googlecode.com/svn/trunk/OpenCVAR morethantechnical-OpenCVAR

Now let your imagination run wild!

Farewell,
Roy.