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

Let’s get down to business…

Getting a motion map

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

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

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

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

Input pair of images, rotation and translation is unknown

Optical Flow

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

Dense O-F using Farneback

Feature Matching

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

SURF features matching, with Fundamental matrix pruning via RANSAC

A hybrid method

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

The rigid transform flow recovered from sparse feature matching

Estimating Motion

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

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

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

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

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

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

Reconstruction via Triangulation

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

"Depth Map"

3D reconstruction

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

Example of when things go bad...

Toolbox and Framework

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

FLTK GUI

Code & Where to go next

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

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

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

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

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

Enjoy!

Roy.

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.

From a template FireBreath plugin to an OpenGL plugin

FireBreath is kind of an amazing project. They aim to be able to write a single source that will create plugins for all browsers and all operating systems. A daunting feat by my book. But building a MacOS Safari/Firefox plugin using their framework proved very simple…
So I started here: http://www.firebreath.org/display/documentation/Mac+Video+Tutorial
It’s a video tutorial of how to create a plugin from template, build it, install it and run it. Follow their instructions and you’ll have your plugin ready in 10 minutes.
The next step will be to make our plugin display an OpenGL scene, which is what OpenNI/NITE use to display their depth map. This was also easy, borrowing code from the FireBreath OpenGL example.
However I ended up with a smaller source since I threw away most of the stuff…

class tutorialpluginMac : public tutorialplugin {
public:
    tutorialpluginMac();
	~tutorialpluginMac();

    BEGIN_PLUGIN_EVENT_MAP()
	EVENTTYPE_CASE(FB::AttachedEvent, onWindowAttached, FB::PluginWindowMac)
	EVENTTYPE_CASE(FB::DetachedEvent, onWindowDetached, FB::PluginWindowMac)
	PLUGIN_EVENT_MAP_CASCADE(tutorialplugin)
    END_PLUGIN_EVENT_MAP()

    virtual bool onWindowAttached(FB::AttachedEvent *evt, FB::PluginWindowMac*);
    virtual bool onWindowDetached(FB::DetachedEvent *evt, FB::PluginWindowMac*);
protected:

private:
    void* m_layer;

};

void glutDisplay (void); //this is implemented in the NITE code

@interface MyCAOpenGLLayer : CAOpenGLLayer {
    GLfloat m_angle;
}
@end

@implementation MyCAOpenGLLayer

- (id) init {
    if ([super init]) {
        m_angle = 0;
    }
    return self;
}

- (void)drawInCGLContext:(CGLContextObj)ctx pixelFormat:(CGLPixelFormatObj)pf forLayerTime:(CFTimeInterval)t displayTime:(const CVTimeStamp *)ts {
    //m_angle += 1;
    GLsizei width = CGRectGetWidth([self bounds]), height = CGRectGetHeight([self bounds]);
    GLfloat halfWidth = width / 2, halfHeight = height / 2;

    glViewport(0, 0, width, height);

	glutDisplay(); //let NITE draw it's stuff

    [super drawInCGLContext:ctx pixelFormat:pf forLayerTime:t displayTime:ts];
}

@end

tutorialpluginMac::tutorialpluginMac() : m_layer(NULL) {}

tutorialpluginMac::~tutorialpluginMac()
{
    if (m_layer) {
        [(CALayer*)m_layer removeFromSuperlayer];
        [(CALayer*)m_layer release];
        m_layer = NULL;
    }
}

bool tutorialpluginMac::onWindowAttached(FB::AttachedEvent* evt, FB::PluginWindowMac* wnd)
{
	cout << "tutorialpluginMac::onWindowAttached" << endl;
    if (FB::PluginWindowMac::DrawingModelCoreAnimation == wnd->getDrawingModel() ||
		FB::PluginWindowMac::DrawingModelInvalidatingCoreAnimation == wnd->getDrawingModel())
	{
        cout << " Setup CAOpenGL drawing. "<<endl;
        MyCAOpenGLLayer* layer = [MyCAOpenGLLayer new];
        layer.asynchronous = (FB::PluginWindowMac::DrawingModelInvalidatingCoreAnimation == wnd->getDrawingModel()) ? NO : YES;
        layer.autoresizingMask = kCALayerWidthSizable | kCALayerHeightSizable;
        layer.needsDisplayOnBoundsChange = YES;
        m_layer = layer;
        if (FB::PluginWindowMac::DrawingModelInvalidatingCoreAnimation == wnd->getDrawingModel())
            wnd->StartAutoInvalidate(1.0/30.0);
        [(CALayer*) wnd->getDrawingPrimitive() addSublayer:layer];
    }
    return tutorialplugin::onWindowAttached(evt,wnd);
}

bool tutorialpluginMac::onWindowDetached(FB::DetachedEvent* evt, FB::PluginWindowMac* wnd)
{
    return tutorialplugin::onWindowDetached(evt,wnd);
}

(You guys will have to fill in the gaps… includes, etc.)

This goes in a new file, a new subclass of the generic plugin, only for Mac. For windows, you should subclass again and create the OpenGL context using WIN32 API or equivalent.

CMakeLists.txt files are also affected. Check out the repo.

NITE OpenGL rendering

Now that the plugin will just draw whatever NITE is drawing, half the battle is done. So for the drawing code I took the simple NiPointViewer example from the NITE library (get it here).
But, since we have need no windows management in the OpenNI, again we can make everything more simple. I took the code exactly as it is, and changed really just a small bit.
I added

#undef USE_GLUT
#undef USE_GLES
, which pretty much makes that code compile to a very lean code (without window management etc.).
And I rescued the glOrtho call in glutDisplay()

//#ifdef USE_GLUT
glOrtho(0, mode.nXRes, mode.nYRes, 0, -1.0, 1.0);
#if defined(USE_GLES)
glOrthof(0, mode.nXRes, mode.nYRes, 0, -1.0, 1.0);
#endif


But the rest is pretty much identical.

One more thing, we should start the Kinect driver and OpenNI stack from somewhere in the plugin loading steps. In the main.cpp file from NITE I changed the main() function to kinect_main().
I did that by adding it here, in the generic plugin (not the Mac subclass because it should be called from all OSs):

bool tutorialplugin::onWindowAttached(FB::AttachedEvent *evt, FB::PluginWindow *)
{
    // The window is attached; act appropriately
	kinect_main(0, 0);
	cout << "tutorialplugin::onWindowAttached" << endl;
    return true;
}

It now will fire when a window is attached to the plugin. The OpenGL calls will start running after the OGL context is up and starts rendering in a loop.

Source and stuff

Get the source for the Kinect-FireBreath plugin at GitHub: https://github.com/royshil/KinectPlugin

This is how it looks:

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

Vertex shader

varying vec3 normal;

void main()
{
    gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
    normal = gl_NormalMatrix * gl_Normal;
}

Fragment shader

varying vec3 normal;

void main()
{
    vec3 normal_normal = normalize(normal);
	gl_FragColor = vec4(normal_normal, 1.0);
}

A technique to load the shaders that will save you a lot of headaches

GLvoid* my_program;

//Error-checking function
void checkARBError(GLvoid* obj) {
	char infolog[1024] = {0}; int _written = 0;
	glGetInfoLogARB(obj, 1024, &_written, infolog);
	if(_written>0) {
		cerr << infolog << endl;
	}
}	

bool notIsAscii(int i) { return !isascii(i); }

void init_shaders() {
	const GLubyte* lang_ver = glGetString(GL_SHADING_LANGUAGE_VERSION);
	cout <<"shading language version: "<<(uchar*)lang_ver<<endl;

	const char * my_fragment_shader_source;
	const char * my_vertex_shader_source;

        //Reading shaders from files
	ifstream ifs("vshader.txt");
	ostringstream ss; ss << ifs.rdbuf();
	ifstream ifs1("fshader.txt");
	ostringstream ss1; ss1 << ifs1.rdbuf();
	ifs.close(); ifs1.close();

        //Cleaning up the strings...
	string _vertex = ss.str(); _vertex.erase(remove_if(_vertex.begin(), _vertex.end(), notIsAscii), _vertex.end());
	string _frag = ss1.str(); _frag.erase(remove_if(_frag.begin(), _frag.end(), notIsAscii), _frag.end());

	// Get Vertex And Fragment Shader Sources
	my_fragment_shader_source = _frag.c_str();
	my_vertex_shader_source = _vertex.c_str();

        //DEBUG - can remove
	cout << "vertex shader:"<<endl<<my_vertex_shader_source<<endl;
	cout << "fragment shader:"<<endl<<my_fragment_shader_source<<endl;

	GLvoid* my_vertex_shader;
	GLvoid* my_fragment_shader;

	// Create Shader And Program Objects
	my_program = glCreateProgramObjectARB();
	my_vertex_shader = glCreateShaderObjectARB(GL_VERTEX_SHADER_ARB);
	my_fragment_shader = glCreateShaderObjectARB(GL_FRAGMENT_SHADER_ARB);

	// Load Shader Sources
	glShaderSourceARB(my_vertex_shader, 1, &my_vertex_shader_source, NULL);
	checkARBError(my_vertex_shader);
	glShaderSourceARB(my_fragment_shader, 1, &my_fragment_shader_source, NULL);
	checkARBError(my_fragment_shader);

	// Compile The Shaders
	glCompileShaderARB(my_vertex_shader);
	checkARBError(my_vertex_shader);
	glCompileShaderARB(my_fragment_shader);
	checkARBError(my_fragment_shader);

	// Attach The Shader Objects To The Program Object
	glAttachObjectARB(my_program, my_vertex_shader);
	glAttachObjectARB(my_program, my_fragment_shader);
	checkARBError(my_program);

	// Link The Program Object
	glLinkProgramARB(my_program);
	checkARBError(my_program);

}

I based it on this example from NeHe.

It does periodical error checking so you can see if something is wrong, plus it will make sure the vertex shader and fragmetn shader are stripped of all non-ASCII characters.
This way the compilation will not give you cryptic errors such as “ERROR: 0:1: ‘<‘ : syntax error syntax error”…

The only things I was interested were the head position and hands positions so I created a struct to hold these, plus some things OpenNI need to get the positions. I did this so it could be run in a different thread, and this struct can be the shared memory:

struct openni_stuff {
	xn::DepthGenerator* dg;
	xn::UserGenerator* ug;
	xn::Context* ctx;
	XnSkeletonJointPosition* Head;
	XnSkeletonJointPosition* rh;
	XnSkeletonJointPosition* lh;
};

All these must be populated in the main before starting the thread

xn::Context g_Context;
xn::DepthGenerator g_DepthGenerator;
xn::UserGenerator g_UserGenerator;
XnSkeletonJointPosition Head;
XnSkeletonJointPosition lHand;
XnSkeletonJointPosition rHand;

int main(..) {
..
g_Context.Init();
g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
..
g_UserGenerator.Create(g_Context);
..
g_Context.StartGeneratingAll();
..
DWORD threadid;
struct openni_stuff s;
s.ctx = &g_Context;
s.dg = &g_DepthGenerator;
s.ug = &g_UserGenerator;
s.Head = &Head;
s.rh = &rHand;
s.lh = &lHand;
CreateThread(
            NULL,                   // default security attributes
            0,                      // use default stack size
			MyThreadFunction,       // thread function name
            (LPVOID)(&s),          // argument to thread function
            0,                      // use default creation flags
            &threadid);   // returns the thread identifie

glutMainLoop();

This code is very abstracted, there are more things to do in order for it to work, you can see them in the code repo.
But basically the new thread is the one getting the information off the OpenNI framework and keeps the head position and hands positions vectors updated.

DWORD WINAPI MyThreadFunction( LPVOID lpParam ) {
       //Unpack the struct, don't care for shallow copy since it's all pointers anyway
	struct openni_stuff s = *((struct openni_stuff*)lpParam);
	for(;;) {
		getOpenNIData(s);
		Sleep(30);
	}
	return 0;
}

void getOpenNIData (struct openni_stuff s)
{
	xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	s.dg->GetMetaData(depthMD);

	if (!g_bPause)
	{
		// Read next available data
		s.ctx->WaitAndUpdateAll();
	}

	// Process the data
	g_DepthGenerator.GetMetaData(depthMD);
	s.dg->GetMetaData(depthMD);
	rHand.position.X = NULL;
	s.ug->GetUserPixels(0, sceneMD);
	DrawDepthMap(depthMD, sceneMD, *s.Head, *s.rh, *s.lh);
}

I thought this will give a performance boost as the WaitAndUpdateAll() call usually takes a little while, but it didn’t matter much…

The OpenGL (GLUT) runs on the main thread, and just looks at these updated vectors for the current position.

Off-Axis projection

The concept of off-axis projection is very important for this project. This very good article explains everything about generalized perspective projections, it also includes C code!, I recommend reading it. But basically off-axis projection is when the viewing eye is not perpendicular the projection surface, nor it needs to be centered in relation to it. It’s what goes on in our human binocular vision, each eye looks at the same point but they are not perpendicular to the virtual projection surface (they are angled to it), and they both have an offset from the center. Just read that little paper….

Anyway, cutting to the chase, we need to project the rendered objects in the scene onto the projection table, assuming the user is not looking at it perpendicularly (like they would with a normal screen). Thanks to the code in the aforementioned article – this is a breeze.

void subtract(float u[3], float v[3], float n[3]) {
	u[0] = v[0] - n[0];
	u[1] = v[1] - n[1];
	u[2] = v[2] - n[2];
}

void projection( float *pa,
				float *pb,
				float *pc,
				float *pe, float n, float f)
{
	float va[3], vb[3], vc[3];
	float vr[3], vu[3], vn[3];
	float l, r, b, t, d, M[16];

	// Compute an orthonormal basis for the screen.
	subtract(vr, pb, pa);
	subtract(vu, pc, pa);

	glmNormalize(vr);
	glmNormalize(vu);
	glmCross(vr, vu, vn);
	glmNormalize(vn);

	// Compute the screen corner vectors.
	subtract(va, pa, pe);
	subtract(vb, pb, pe);
	subtract(vc, pc, pe);

	// Find the distance from the eye to screen plane.
	d = -glmDot(va, vn);

	// Find the extent of the perpendicular projection.
	l = glmDot(vr, va) * n / d;
	r = glmDot(vr, vb) * n / d;
	b = glmDot(vu, va) * n / d;
	t = glmDot(vu, vc) * n / d;
	// Load the perpendicular projection.
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glFrustum(l, r, b, t, n, f);
	// Rotate the projection to be non-perpendicular.
	memset(M, 0, 16 * sizeof (float));
	M[0] = vr[0]; M[4] = vr[1]; M[ 8] = vr[2];
	M[1] = vu[0]; M[5] = vu[1]; M[ 9] = vu[2];
	M[2] = vn[0]; M[6] = vn[1]; M[10] = vn[2];
	M[15] = 1.0f;
	glMultMatrixf(M);
	// Move the apex of the frustum to the origin.
	glTranslatef(-pe[0], -pe[1], -pe[2]);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
}

I am using glm.h & glm.c from Nate Robbins to do some basic lin-algebra. I just didn’t feel like re-writing the code, and I’m already using it to load Wavefront OBJ models. The only missing function is subtract which is included.

Loading the OBJ models is super easy with glm.h:

	   objmodel_ptr = glmReadOBJ("../bunny1.obj");
	   if (!objmodel_ptr)
		   exit(0);

	   glmUnitize(objmodel_ptr);
	   glmFacetNormals(objmodel_ptr);
	   glmVertexNormals(objmodel_ptr, 90.0);

Now that we can create off-axis views (this can be reused for other projects, such as projects with VR glasses!), I draw the scene after applying this projection:

GLfloat eye[4] = {0,200,1050,0}; //position of eye
double kinectHeight = 300;  //the Kinect is by the table, at a certain height (measured)
GLdouble tlv[3] = {-530, -kinectHeight, 90},   //top-left point of table in Kinect coordinates (millimeters)
		trv[3] = {530, -kinectHeight, 90}, //top-right
		brv[3] = {530, -kinectHeight, 955}, //bottom-right
		blv[3] = {-530, -kinectHeight, 955}; //bottom-left
GLdouble obj[3] = {-200, tlv[1], 522.5}; //the virtual object's real-world position (mm)

static void display(GLenum mode)
{
       //set the eye position
	if(Head.position.X != 0.0f || Head.position.Y != 0.0f || Head.position.Z != 0.0f)
	{
		eye[0] = Head.position.X;
		eye[1] = Head.position.Y;
		eye[2] = Head.position.Z;
	}

	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
	offAxisView();
}

void offAxisView() {
	projection(blvf, brvf, tlvf, eye, 1.0f, 10000.0f);

	glLightfv(GL_LIGHT0, GL_POSITION, lightp);
	drawScene();

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

void drawScene() {
	//Just draw an object..
	glPushMatrix();
	glTranslated(obj[0]-10,obj[1]+80,obj[2]); //translating to accomodate for obj size
	glColor4f(1.0, 0.0, 0.0, 1.0);
	glScaled(80,80,80);
	glmDraw(objmodel_ptr,GLM_SMOOTH);
	glPopMatrix();
}

You can see that I measured the position of the table in respect to the Kinect sensor’s center, we assume that it is the origin, and these are used for the off-axis projection w.r.t the eye.

That’s pretty much it… the program runs, you have to stand in the silly “Psi” position for the OpenNI framework to calibrate, and then the graphics will be rendered according to your head position.

To create your own setup, just put in the right position of the table in respect to the Kinect sensor in real-world coordinates (mm).

Code

Can be downloaded from SVN as usual:
svn co https://morethantechnical.googlecode.com/svn/trunk/kinect_motion_parallax/main.cpp

Video

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

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

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

Here’s how it’s done

Intro

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

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

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

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

Time to Code

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

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

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

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

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

vector<Point2f > imagePoints;

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

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

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

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

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

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

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

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

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

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

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

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

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

That wasn’t too hard, huh? Awesome.

So…. Results

Code

You can grab the code from the SVN repo:

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

Enjoy!

Roy.

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

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

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

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

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

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

Preparations…

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

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

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

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

Getting Test Data

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

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

Stereo Initialization

Now that I have 2 views with feature correspodence:

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

Triangulation

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

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

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

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

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

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

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

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

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

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

}

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

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

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

	CvMat __points1 = ___tmp, __points2 = ___tmp1;

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

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

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

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

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

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

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

	status = vector<uchar>(oc);

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

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

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

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

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

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

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

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

	}
}

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

Pose Estimation

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

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

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

	Mat rvec(rv),tvec(tv);

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

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

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

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

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

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

Integrating OpenGL

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

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

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

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

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

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

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

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

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

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

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

	glutSwapBuffers();

	if(!running) {
		glutLeaveMainLoop();
	}

	Sleep(25);
}

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

Things not covered in this work

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

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

Code and Working the Program

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

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

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

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

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

Bye!

Roy.

In the last weekend I attended GeekCon 2009, a tech-conference, with a friend and colleague Arnon (not Arnon from the blog, who recently had a birthday – Happy B-Day Arnon!). Each team that attended had to create a project they can complete in 2-days of the conference. Our project is called “RunVas”, and the basic idea was to let people run around and paint by doing so. We wanted to combine computer vision with a little artistic angle.

Here’s some more details

GeekCon you say?

First of all a few words about GeekCon itself. The conference is a “non-conference” or “un-conference”, which is a conference not focused on the business side of innovation and technology, but on the fun and creative side. The moto is something like: “geek out as hard as you possibly can in 2 days, and get it out of your system for the rest of the year”.

So teams from all corners of technology: Elect. Eng., Comp. Sci., Metal and wood works, etc., register and state their project of choice. The managment decides whether the project can actually be delivered in 2 days, and is actually a “GeekCon project”. By “GeekCon project” they mean something that demonstrates a nice concept/idea in a cool way, and is utterly useless in real life. This is the official stand.

Our project

We were accepted in with our project, RunVas. A simple idea, based around the latest fashion of getting people out of the house, away from the computer and hit the lawns running. We wanted also to combine technical and artistic point-of-views. So we create a system that tracks objects in a video scene, and sends the results to a drawing engine. The drawing will be presented on a virtual “canvas”, that the runners can view as they run, hence the name “RunVas”. We weren’t able to achieve all of that, but we had a good go at it, and delivered something nice.

Implementation

The CV part, object tracking, was programmed by Arnon, using the archaic Macromedia Director, donno which version but an old one anyway. And the drawing part was created by myself using the groundwork I had done for my 3D graphics game I programmed for school using SWT/JOGL. Personally I was amazed by how quickly I was able to pick up the framework from that project and re-use it for another, completely different, project. I guess that if you write stuff in a good solid structure you can build anything on top of it.

Media

So without further ado, here’s a short video:

And my flickr stream with photo I uploaded in real time from the conference:

Code

The code for the canvas drawing proggy is available in the SVN repo.

Thanks!
Roy.