More Than Technical » video

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

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

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

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

So the algorithm is basically

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

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

Finding closest points

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

		ShowQuery(destination,X,X_bar);

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

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

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

Thanks for joining!

Roy.

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

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

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

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

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

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

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

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

Preparations…

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

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

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

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

Getting Test Data

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

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

Stereo Initialization

Now that I have 2 views with feature correspodence:

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

Triangulation

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

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

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

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

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

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

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

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

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

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

}

void triangulate(vector& points1, vector& points2, vector& status) {

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

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

	CvMat __points1 = ___tmp, __points2 = ___tmp1;

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

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

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

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

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

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

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

	status = vector(oc);

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

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

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

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

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

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

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

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

	}
}

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

Pose Estimation

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

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

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

	Mat rvec(rv),tvec(tv);

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

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

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

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

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

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

Integrating OpenGL

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

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

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

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

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

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

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

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

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

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

	//draw the 3D feature points
	glPushMatrix();
	glColor4d(1.0,0.0,0.0,1.0);
	for(unsigned int i=0;i
This pretty much coveres my work, in a very concise way. The complete source code will reveal all I have done, and will provide a better copy-and-paste ground for your own projects.
Things not covered in this work
Initially I tried to implement a very crucial part of the PTAM work – pairing the 3D map with 2D features in the image. This allows them to re-align the map in every frame (when the tracking is bad) so the pose estimation does not “loose grip”. In essence, they keep a visual identity for each map feature, very similar to a descriptor like SURF or SIFT, so at any point they can find where in the new image are the features and recover the camera pose from the 2D-3D correspondence. I ran into a problem utilizing OpenCV’s SURF functionality, it seems to have a bug when trying to compute the descriptor for user-given feature points.
Another thing I chose not to implement is creating a full map of the surroundings. I wanted to achieve a simple working solution for a small map (essentially a single frame), and see how it works. In the original work by K&M they constantly add more and more features to the map untill it has covered the whole surrounding room.
Code and Working the Program
As usual my code is available for checkout from the blog’s SNV repo:
svn checkout http://morethantechnical.googlecode.com/svn/trunk/ptam ptam

To get the stereo initialization you must press [spacebar] twice: Once when the camera has stabilized and the features are stable, and another time when the camera has translated and again stabilized.

This marks the 2 keyframes that will be used for stereo init and triangulation.

From that point on, the 3D scene will start and the track-and-estimate stage begins. Try not to move the camera violently as the optical flow may suffer.
Thanks Lior for your help getting the hang of these subjects, and the opportunity to meddle with a subject I long gone wanted to explore.
I hope everyone will enjoy and learn from my enjoyment and learning.
Bye!
Roy.

iPhone OS 3.x Raw data of camera frames

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

Hi All

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

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

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

Way to go John!
Some code would be awesome…

Roy.

SmartHome – Embedded computing course project

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

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

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

The Idea

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

Such a system will be able to automatically:

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

The Hardware

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

The Software

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

Embedded program

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

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

All commands / responses have a unified structure described here:

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

Data:

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

OPERATE
_ _________
 | BITMASK |
_|_________|

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

Communication with ZigBee module

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

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

The master station logic is like so:

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

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

Communication with PC

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

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

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

“up=”, the station indexed i is up

“master_online”,

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

“e=”, error message from the master

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

PC client program

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

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

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

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

The Code

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

It is available via the blog’s SVN repo:

PC Client (requires RxTx serial port impl and SWT)

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

Embedded program (includes all dependencies)

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

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

Java is compiled as usual… just remember the dependencies.

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

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

GeekCon 2009: RunVas – Our project [w/ video, img]

Roy — Tue, 13 Oct 2009 09:14:49 +0000

Hi everyone

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.

Awesome pictures fusing with a GIMP plugin [w/ code]

Roy — Mon, 21 Sep 2009 12:58:58 +0000

Switching, merging or swapping, call it what you like – it’s a pain to pull off. You need to spend a lot of time tuning the colors, blending the edges and smudging to get a decent result. So I wrote a plugin for the wonderful GIMP program that helps this process. The merge is done using a blending algorithm that blends in the colous from the original image into the pasted image.

I’ll write a little bit about coding GIMP plugins, which is very simple, and some about the algorithm and its sources.

Let’s see how it’s done

Writing a GIMP plugin

As I mentioned, writing a plugin for the GIMP is not very hard. I used the excellent (though outdated) guide for writing a plugin from GIMP.org.
There are 2 kinds of plugins: scripts, and “native” plugins. Script plugins are written in script-fu, the scripting language for GIMP, they are not compiled. On the opposite side, the “native” plugins are written either in C and compiled to executable, or in Python or Perl. I wrote my plugin in C based roughly on the plugin template, and compiled both on Linux and windows.

From the ground up

The first step I did was getting my hands on the pixel data of the selected layer. This is covered pretty nicely in the tutorial.
Once you get a GimpDrawble you just use gimp_pixel_rgn_init, and then gimp_pixel_rgn_get_row to get a whole row of pixel data (interleaved RGB/RGBA).

To access elements in the row of bytes you need to know how many channels are in the interleave.

gint channels, x1, y1, x2, y2, x_off, y_off, m, n; GimpPixelRgn rgn_in; guchar *row; gimp_drawable_mask_bounds (drawable->drawable_id, &x1, &y1, &x2, &y2); gimp_drawable_offsets(drawable->drawable_id,&x_off,&y_off); m = y2 - y1; //height of relevant region n = x2 - x1; //width of relevant region channels = gimp_drawable_bpp (drawable->drawable_id); gimp_pixel_rgn_init (&rgn_in, drawable, x1, y1, n, m, FALSE, FALSE); row = g_new (guchar, channels * n); for (int y = 0; y < m; y++) { gimp_pixel_rgn_get_row (&rgn_in, row, 0, y, n); //manipulate pixels... }

Working with layers masks is also simple. Masks are GimpDrawables as well.

gint32 mask_id = gimp_layer_get_mask(drawable->drawable_id); GimpDrawable* mask_drawable = gimp_drawable_get(mask_id);

Progress reporting

During the run of a lengthy operation it’s recommended to notify the user of the progress. The easiest way to do it in a GIMP plugin is use the built-in progress bar. Very easy to use:

gimp_progress_init ("Clone..."); .... gimp_progress_set_text_printf("Create matrix %dx%d",mn,mn); gimp_progress_update(0.33); .... gimp_progress_update(1.0);

User inputs

User input for plugins can be easily done using GTK dialogs. The gimp plugin development tutorial is covering this briefly, but its fairly simple for someone who is already fimiliar with GTK. I copied off the tutorial, and changed the spinbutton to a checkbutton for my needs.

One tip: in the tutorial code the gimp_dialog_new function will not accept just an int for the 4th parameter (needs GtkDialogFlags), so I passed in GtkDialogFlags::GTK_DIALOG_MODAL.

Now that we got the bases covered, let’s move on to the algorithm.

The blending algorithm

The algorithm is based on a paper published in 03′ by Microsoft Research. The article presents a way to merge two images by inspecting their gradients, and using this information to “bleed” in colors from the background image into the pasted image. More generally, the colors from the outer image (the background) will seep into the pasted image, and they will do it faster if the pasted image is more smooth. This creates a very nice blending effect.

There’s some mathematical mumbo-jumbo about estimating and discretising the calculation of the gradient, and in the end it boils down to a simple sparse matrix multiplication. I used GMM++ library to do this in C environment. The library compiles from source (only .h files), so you only need to #include it and set the project paths correctly.

How to use the plugin

OK, so all this BS about GIMP, C and math gave me a headache, and we’re here to have some fun after all. So without further ado, let’s see how to use this plugin to do some nice tricks. Originally I used this plugin to merge faces together, but it actually can be used to merge anything.

For example, a polar bear in the desert:

This is how to do it:

get a background picture, preferably one with a uniform color part on which you’ll want to blend in the external object

get an image of an object, preferably surrounded by a uniform color “buffer” so it will blend nicely into the background

if needed, cut out a bounding rectangle of the object with a good margin on all sides, and place it where you want on the background

create a mask, and use lasso to create a tighter mask around the object

select the object layer, and fire the plugin

Some other stuff I did:

Video time…

Source of course

Source is as usual in our Google Code SVN repo.

Executable (win32) is available here.

Thank you for tuning in!
Roy.

iPhoneOS 3.1 will not allow marker-based AR

Roy — Wed, 26 Aug 2009 14:01:39 +0000

Hi

I had very high hopes for iPhoneOS 3.1 in the AR arena. With all the hype about it, I naturally thought that with 3.1 developers will be able to bring marker-detection AR to the app-store – meaning, using legal and published APIs. A look around 3.1′s APIs I wasn’t able to find anything that will allow this.

Not all AR is banned. In fact AR apps like Layar will be very much possible, as they rely on compass & gyro to create the AR effect. These don’t require processing the live video feed from the camera, only overlaying data over it. This can be done easily with the new cameraOverlayView property of UIImagePickerController. All you need to do is create a transparent view with the required data, and it will be overlaid on the camera preview.

Sadly, to get marker-detection abilities developers must still hack the system (camera callback rerouting), or use very slow methods (UIGetScreenImage). I can only hope apple will see the potential of letting developers manipulate the live video feed.

Roy.

Near realtime face detection on the iPhone w/ OpenCV port [w/code,video]

Roy — Sun, 09 Aug 2009 11:45:05 +0000

Hi
OpenCV is by far my favorite CV/Image processing library. When I found an OpenCV port to the iPhone, and even someone tried to get it to do face detection, I just had to try it for myself.

In this post I’ll try to run through the steps I took in order to get OpenCV running on the iPhone, and then how to get OpenCV’s face detection play nice with iPhoneOS’s image buffers and video feed (not yet OS 3.0!). Then i’ll talk a little about optimization

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

Let’s begin

Cross compiling OpenCV on iPhoneOS

The good people @ computer-vision-software.com have posted a guideline on how to compile OpenCV on iPhone and link them as static libraries, and I followed it. I did have to recompile it with one change – OpenCV needed zlib linkage, and the OpenCV configure script wasn’t able to config the makefiles to compile zlib as well. So I downloaded zlib from the net, and just added all the files to the XCode project to compile and link. If you’re trying to recreate this, remember to configure/build zlib before adding the files to XCode so you get a zconf.h file. Now OpenCV linked perfectly.
All in all it was really not a big deal to compile OpenCV to the iPhoneOS. I imagined it will be much harder…

OK moving on to

Plain vanilla face detection

So the first step is to just get OpenCV to detect a single face on a single image. But let’s make it harder and use UIImage.
So first, I took OCV’s facedetect.c example, and added it to the project as is. Then I add 2 peripheral functions to setup and tear down the structs and allocated static memory (things that are done in the main function).

void init_detection(char* cascade_location) { cascade = (CvHaarClassifierCascade*)cvLoad( cascade_location, 0, 0, 0 ); storage = cvCreateMemStorage(0); } static IplImage *gray = 0, *small_img = 0; void release_detection() { if (storage) { cvReleaseMemStorage(&storage); } if (cascade) { cvReleaseHaarClassifierCascade(&cascade); } cvReleaseImage(&gray); cvReleaseImage(&small_img); }

The detect_and_draw function remains exactly the same at this point. I just take the XML files of the haarcascades, and add them to the projects resources.
Now I initialize the detection structs from my UIView or UIViewController that will do the detection. The main NSBundle will find the path to the XML file:

NSString* myImage = [[NSBundle mainBundle] pathForResource:@"haarcascade_frontalface_alt" ofType:@"xml"]; char* chars = (char*)malloc(512); [myImage getCString:chars maxLength:512 encoding:NSUTF8StringEncoding]; init_detection(chars);

Awesome, now let’s face-detect already! For that all we need is to attach a picture of someone to the projects resources, load it, convert it to IplImage* and hand it over to detect_and_draw – simple.
I used a couple of helper function from the informative post I mentioned earlier:

- (void)manipulateOpenCVImagePixelDataWithCGImage:(CGImageRef)inImage openCVimage:(IplImage *)openCVimage; - (CGContextRef)createARGBBitmapContext:(CGImageRef)inImage; - (IplImage *)getCVImageFromCGImage:(CGImageRef)cgImage; -(CGImageRef)getCGImageFromCVImage:(IplImage*)cvImage;

Now it’s only putting it together:

IplImage* im = [self getCVImageFromCGImage:[UIImage imageNamed:"a_picture.jpg"].CGImage]; detect_and_draw(im); UIImage* result = [UIImage imageWithCGImage:[self getCGImageFromCVImage:im]]; UIImageView* imv = [[UIImageView alloc] initWithImage:result]; [self addSubview:imv]; [imv release];

Just remember those externs, if you don’t use a header file:

extern "C" void detect_and_draw( IplImage* img, CvRect* found_face ); extern "C" void init_detection(char* cascade_location); extern "C" void release_detection();

Sweet. But detecting a face on a single photo is not so difficult – we want video and real-time face detection! So let’s do that..

Tying it up with video feed from the iPhone camera (no OS 3.0 yet)

This step was so amazingly simple, it was borderline funny. I used my well-known camera frame grabbing code from Norio Numora. Of course to align it with OS 3.0 you must plug it in to the API Apple provide, and not this wily hack, but it’s really a plug-and-play situation. I use it in many of my projects that use the iPhone camera, untill video on the OS 3.0 will be finalized.
So all I needed was to set everything up, make a timer to fire every so-and-so millisec, and send the frame to detection:

- (id)initWithNibName:(NSString *)nibNameOrNil bundle:(NSBundle *)nibBundleOrNil { if (self = [super initWithNibName:nibNameOrNil bundle:nibBundleOrNil]) { // Initialization code ctad = [[CameraTestAppDelegate alloc] init]; [ctad doInit]; NSString* myImage = [[NSBundle mainBundle] pathForResource:@"haarcascade_frontalface_alt" ofType:@"xml"]; char* chars = (char*)malloc(512); [myImage getCString:chars maxLength:512 encoding:NSUTF8StringEncoding]; init_detection(chars); [self.view addSubview:[ctad getPreviewView]]; [self.view sendSubviewToBack:[ctad getPreviewView]]; repeatingTimer = [NSTimer scheduledTimerWithTimeInterval:0.0909 target:self selector:@selector(doDetection:) userInfo:nil repeats:YES]; } -(void)doDetection:(NSTimer*) timer { if([ctad getPixelData]) { if(!im) { im = cvCreateImageHeader(cvSize([ctad getVideoSize].width,[ctad getVideoSize].height), 8, 4); } cvSetData(im, [ctad getPixelData],[ctad getBytesPerRow]); CvRect r; detect_and_draw(im,&r); if(r.width > 0 && r.height > 0) { NSLog(@"Face: %.0f,%.0f,%.0f,%.0f",r.x,r.y,r.width.r.height); } } }

See that for optimization sake, I only create the IplImage header once (the if goes in only in the first time), and every frame after that I only set the IplImage data by taking the buffer I got from the camera. This way the IplImage is sharing buffers, so there is also a little memory optimization there.
From that point on you can take it anywhere you like. Add stuff to faces, mark the face in the image, etc.

But… there’s the issue of performance. This method will get you very very bad timings. In the area of 5-15 seconds (!!) for a single frame – which is horrendous. And I promised near real time performance. So without further ado,

Optimizing the hell out of the detection algorithm

Well the guys at computer-vision-software.com have done some work in the field of optimizing OpenCV’s haar-based detection, but never released code. Their method was based on the fact that the iPhone’s CPU can handle integers far better than floating-points, so they set out to change the algorithm to use integers. I also did that, and found that it only shaves off a few millisec of the total time. The far more influencing factor is the window size of the features scan, the scaling factor of the window size, and the derived number of passes.

Let me explain a little bit how the detection works in OpenCV. First you set the minimal size of the window. Then you specify a scale factor. OpenCV uses this scale factor to do multiple passes over the image to scan for feature-hits. It take the window size, say 30×30, and the factor, say 1.1, and starts multiplying the window size by the factor until it reaches the size of the image. So for a 256×256 image you get: 30×30 scan, 33×33, 36×36, 39×39, 43×43… 244×244 – a total of 23 passes, for one frame! This is way too much… This is done to get better and finer results, and it may be good for resource abundant systems, but this is not our case.

So first thing I did was slash down on those scans. There is, as expected a very strong impact on the quality of the results, but the times are getting close to acceptable. After all my optimizations I got the timing down to even ~120ms.
I optimized a few things:

The size of the input image, originally ~300×400, was cut down by 1.5

The scale factor for cvHaarDetectObjects: I played with values ranging from 1.2 to 1.5, with pleasing timings

The ROI (region of interest) in the IplImage to scan was set every frame to have the previous frame’s detection, the location of the face, plus some buffer on the sides to allow movement of the face frame-to-frame. This decreases the scanned area from the whole image to just a small portion that contains the known face. Of course if a face was not found the ROI is reset.

I change the internal works of the cvHaarDetectObjects algorithm to do a lot less floats multiplications and turned them into integer multiplications.

I dawned upon me just the other day that I can also optimize the size of the search window, and not keep it constant from frame to frame (30×30). If the last frame had found a 36×36 face, the next detection should also try for a 36×36 object. I haven’t tried it yet.

Memory optimization: don’t alloc buffers every frame, share buffers, etc.

So first the most influential change, is in the detection phase:

void detect_and_draw( IplImage* img, CvRect* found_face ) { static CvRect prev; if(!gray) { gray = cvCreateImage( cvSize(img->width,img->height), 8, 1 ); small_img = cvCreateImage( cvSize( cvRound (img->width/scale), cvRound (img->height/scale)), 8, 1 ); } if(prev.width > 0 && prev.height > 0) { cvSetImageROI(small_img, prev); CvRect tPrev = cvRect(prev.x * scale, prev.y * scale, prev.width * scale, prev.height * scale); cvSetImageROI(img, tPrev); cvSetImageROI(gray, tPrev); } else { cvResetImageROI(img); cvResetImageROI(small_img); cvResetImageROI(gray); } cvCvtColor( img, gray, CV_BGR2GRAY ); cvResize( gray, small_img, CV_INTER_LINEAR ); cvEqualizeHist( small_img, small_img ); cvClearMemStorage( storage ); CvSeq* faces = mycvHaarDetectObjects( small_img, cascade, storage, 1.2, 0, 0 |CV_HAAR_FIND_BIGGEST_OBJECT |CV_HAAR_DO_ROUGH_SEARCH //|CV_HAAR_DO_CANNY_PRUNING //|CV_HAAR_SCALE_IMAGE , cvSize(30, 30) ); if(faces->total>0) { CvRect* r = (CvRect*)cvGetSeqElem( faces, 0 ); int startX,startY; if(prev.width > 0 && prev.height > 0) { r->x += prev.x; r->y += prev.y; } startX = MAX(r->x - PAD_FACE,0); startY = MAX(r->y - PAD_FACE,0); int w = small_img->width - startX - r->width - PAD_FACE_2; int h = small_img->height - startY - r->height - PAD_FACE_2; int sw = r->x - PAD_FACE, sh = r->y - PAD_FACE; prev = cvRect(startX, startY, r->width + PAD_FACE_2 + ((w < 0) ? w : 0) + ((sw < 0) ? sw : 0), r->height + PAD_FACE_2 + ((h < 0) ? h : 0) + ((sh < 0) ? sh : 0)); printf("found face (%d,%d,%d,%d) setting ROI to (%d,%d,%d,%d)\n",r->x,r->y,r->width,r->height,prev.x,prev.y,prev.width,prev.height); found_face->x = (int)((double)r->x * scale); found_face->y = (int)((double)r->y * scale); found_face->width = (int)((double)r->width * scale); found_face->height = (int)((double)r->height * scale); } else { prev.width = prev.height = found_face->width = found_face->height = 0; } }

As you can see I keep the previous face in prev, and use it to set the ROI of the images for the next frame. Note that the small_img is a scaled-down version of the input image, so the detection results must be scaled-up to match the real size of the input.

Now, I can bore you with the details of how I changed the cvHaarDetectObjects to use more integers, but I won’t. Anyway it’s all in the code, that is freely available, so you can diff it against cvHarr.cpp of OpenCV and see the changes. In short what I did was:

Mark out image scaling and canny pruning.

in the cvSetImagesForHaarClassifierCascade, which fires many times for each frame and is governed on scaling/shifting/rotating the Haar classifiers to get better detection, I changed the weights and sizes to be integers rather than floats.

in cvRunHaarClassifierCascade, which calculates the score for a single Haar feature-hit, I changed the results calculation to integers instead of floats.

I played around with integer oriented calculations of the sqrt function, that the cvRunHaarClassifierCascade func uses (fires many many times each frame), but that actually caused a slow-down on the device. Turns out the standard library (math.h) implementation is the best

Well guys, that’s pretty much all my discovery in the field. Please keep working on it. I’m anxious to see a true real-time face detection on the iPhone.

Time for a video proof? you bet

Here’s proof that all I wrote here is not total BS

Code

Code is as usual available in Google Code SVN repo:
http://code.google.com/p/morethantechnical/source/browse/#svn/trunk/FaceDetector-iPhone

OK, ‘Till next time, enjoy
Roy.

Advanced topics in 3D game building [w/ code, video]

Roy — Mon, 27 Jul 2009 12:17:27 +0000

Hi

The graphics course I took at TAU really expanded my knowledge of 3D rendering, and specifically using OpenGL to do so. The final task of the course, aside from the exam, was to write a 3D game. We were given 3 choices for types of games: worms-like, xonix-like and lightcycle-like. We chose to write our version of Worms in 3D.

I’ll try to take you through some of the problems we encountered, the decisions we made, and show as much code as possible. I’m not, however, gonna take you through the simple (yet grueling) work of actually showing meshes to the screen or moving them around, these subjects are covered extensively online.

The whole game is implemented in Java using JOGL and SWT for 3D rendering. The code is of course available entirely online.

In the begining there was mesh

Our game is a turn-based shooter, Worms-like. The players are supposed to walk on the face of a world mesh (a 3d structure), and shoot missiles at each other.
When it comes to positioning and aligning 3D objects on top of other 3D objects, I must say I was overwhelmed by the complexity. Its really not that easy to do, but once you get the idea its easy.
So you have an arbitrary 3D object, and you want another object to “walk” over it. Few things you need to know:

the normal of the mesh at any given point, so your player can have an “up” direction (away from the mesh)

the global orientation of your player object, so when you align it up it will actually face up

Now, to align the object you have to make it be oriented upwards from the mesh and point in the forward direction. So before drawing the player object make the model-view matrix face the right direction. For this we ported some CPP code that does exactly that:

public static void multLookAt (/*float eyex, float eyey, float eyez, float atx, float aty, float atz, float upx, float upy, float upz,*/ Vector3D origin, Vector3D forward, Vector3D up, GL gl) { float m[] = new float[16]; // Compute our new look at vector, which will be // the new negative Z axis of our transformed object. forward = forward.getNormalized(); // Cross product of the new look at vector and the current // up vector will produce a vector which is the new // positive X axis of our transformed object. Vector3D xaxis = forward.crossProduct(up).getNormalized(); m[0] = xaxis.x; m[1] = xaxis.y; m[2] = xaxis.z; // Calculate the new up vector, which will be the // positive Y axis of our transformed object. Note // that it will lie in the same plane as the new // look at vector and the old up vector. up = xaxis.crossProduct(forward); m[4] = up.x; m[5] = up.y; m[6] = up.z; // Account for the fact that the geometry will be defined to // point along the negative Z axis. forward = forward.multiply(-1f); m[8] = forward.x; m[9] = forward.y; m[10] = forward.z; // Fill out the rest of the 4x4 matrix m[3] = 0.f; // xaxis is m[0..2] m[7] = 0.f; // up is m[4..6] m[11] = 0.f; // -at is m[8..10] m[12] = origin.x; m[13] = origin.y; m[14] = origin.z; m[15] = 1.f; // Multiply onto current matrix stack. gl.glMultMatrixf(FloatBuffer.wrap(m)); }

Walk the mesh

Another key point in our game is making the player character walk smoothly on the mesh. The instructions for this exercise were that we have to make the player either walk from vertex to vertex, from edge to edge or the most complicated option – an arbitrary point on the mesh to another. We chose to make the player walk from edge to edge.
Say that the player object is on an edge. To make it walk to another edge we needed to know what are the possible edges that it could reach. For that we created a half-edge structure of the world mesh. Then when we query the structure we can get in constant time the neighboring edges to the player (typically the one-ring of both vertices of the current edge he’s standing on). We only have to choose from the list of edges which edge the player should go to, and where on that edge he will “land”. This is done by choosing the edge with the closest point to the approximation of the players next location.
Let me explain:

The player has a certain speed, meaning it can cover a certain distance in a given time.

The approximate next location of the player is the distance it should cover in minimal time. We took a constant value.

Once you have a approximate location, find the edge (and a point on that edge) that best conforms with this location

In the image you see the player object (snail), the ring of edges around the player that we check (green), the approximate next location (yellow), and the calculated next location (purple).

To find a best location for the object we calculated the distance between the player and the approximate. We ported another piece of C code to measure distance between segments to do that:

/** * find the distance between 2 line segments * @param S1P1 1st point of segment 1 * @param S1P0 2nd pt of seg 1 * @param S2P1 1st pt of seg 2 * @param S2P0 2nd pt of seg 2 * @return */ public static Vector3D dist3D_Segment_to_Segment( Vector3D S1P1, Vector3D S1P0, Vector3D S2P1, Vector3D S2P0) { Vector3D u = S1P1.minus(S1P0); Vector3D v = S2P1.minus(S2P0); Vector3D w = S1P0.minus(S2P0); float a = u.innerProduct(u); // always >= 0 float b = u.innerProduct(v); float c = v.innerProduct(v); // always >= 0 float d = u.innerProduct(w); float e = v.innerProduct(w); float D = a*c - b*b; // always >= 0 float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 // compute the line parameters of the two closest points sN = (b*e - c*d); if (sN < 0f) { // sc < 0 => the s=0 edge is visible return null; } else if (sN > sD) { // sc > 1 => the s=1 edge is visible return null; } // finally do the division to get sc sc = (Math.abs(sN) < Vector3D.EPSILON ? 0f : sN / sD); return S1P0.add(u.multiply(sc)); // return the intersection point }

We actually check what is the intersection point of the approximate edge with the line that goes from the player and in the forward direction. This is the point the player should be if it walked from his current position to that edge.

Orchestrating:

/** * find the location where the player should be in the next frame * @param distanceToMoveInThisInterval the distance the player object should (try to) move * @return */ public EdgeAndIntersectionPointAndDistance findNextLocation(float distanceToMoveInThisInterval) { //find all adjacent edges using half-edge struct Edge currentEdge = model.getCurrentEdge(); Set allEdges = currentEdge.b.get2RingOfEdges(); allEdges.addAll(currentEdge.a.get2RingOfEdges()); //A point that lies a little bit along the line from the player and in the forward direction Vector3D locationAndSome = location.add(dirForward.multiply(0.05f)); //find the edge that the player direction is intersecting allEdges.remove(currentEdge); ArrayList intersectingEdges = new ArrayList(); for (Edge e : allEdges) { //skip the opposite edge too. if((e.a == currentEdge.a && e.b == currentEdge.b) || (e.b == currentEdge.a && e.a == currentEdge.b)) continue; Vector3D intPt = Utils.dist3D_Segment_to_Segment(e.a.getVector3D(), e.b.getVector3D(), model.getLocation(), locationAndSome); if(intPt != null) { float dist = intPt.minus(locationAndSome).getNorm(); intersectingEdges.add( new EdgeAndIntersectionPointAndDistance(e,dist,intPt) ); } } EdgeAndIntersectionPointAndDistance bestEdge = null; float bestDistance = Float.MAX_VALUE; for (EdgeAndIntersectionPointAndDistance ead : intersectingEdges) { float absD = ead.distance; if(absD < bestDistance) { bestDistance = absD; bestEdge = ead; } } //find the location on that edge where the player should be return bestEdge; }

Of course when the player gets to his next location, he needs to get the correct up-direction and forward direction. This can be taken from the next edge’s vertices that hold normals.

There are some additional issues to traversing the mesh:

Avoiding hitting other players. Done by not allowing 2 player to inhabit the same edge.

Collecting bonuses that are on the ground by checking if the player had walked over (or close to) an edge that has a bonus on it

Up keeping the half-edge structure by updating the edges if they have players/bonuses on them

Make a dent in the world

One more interesting issue is the missiles impact on the world. Naturally, we’d like the missiles that hit the ground to make a dent in the world. Our solution was to take the 1-ring of the hit vertex, and lower all the vertices on the ring in the opposite direction to their normals.

public static void makeADentInTheMesh(Vertex vtx, float amount, IWorldMeshHandler worldMeshHandler) { //Make a dent in the mesh... Set es = vtx.get2RingOfEdges(); for (Edge ed : es) { Vector3D v = new Vector3D(ed.a.x,ed.a.y,ed.a.z); v = v.minus(ed.a.getNormal().multiply(amount)); ed.a.x = v.x; ed.a.y = v.y; ed.a.z = v.z; v = new Vector3D(ed.b.x,ed.b.y,ed.b.z); v = v.minus(ed.b.getNormal().multiply(amount)); ed.b.x = v.x; ed.b.y = v.y; ed.b.z = v.z; //If there's stuff on the edge, move it accordingly if(ed.playerOnEdge != null) { ed.playerOnEdge.getModel().fixLocation(); } if(ed.packageOnEdge != null) { Utils.fixObjLocationToEdge(ed.packageOnEdge.getModel(), ed); } if(ed.treeOnEdge != null) { Utils.fixObjLocationToEdge(ed.treeOnEdge.getModel(), ed); } } //Recalculate normals - the positions have changed, creating new "up" directions for (Edge ed : es) { ed.a.calcNormal(); ed.b.calcNormal(); } //reset the display list of the world mesh since the vertices and faces have changed worldMeshHandler.getWorld().getRenderer().setResetList(); }

if pigs (missiles) could fly…

So far I’ve covered mesh oriented movement. Missiles, however, are not mesh-bound – they fly around “freely” above the mesh. To imitate gravity and a “steep course of flight” for the missiles, we use Bezier curves of either 3 or 4 keypoints.
To calc a point on the curve all you need to know is the current time of flight.

protected Vector3D[] mBezierMultiplyV0_V3 = null; protected Vector3D V0; protected Vector3D V1; protected Vector3D V2; protected Vector3D V3; protected void calcExpectedFlyTime() { float distToMove = V3.minus(V0).getNorm(); //total distance to cover expectedFlyTime = distToMove / flySpeed; //approximate time of flight } protected Vector3D getCurrentCurveLocation(float u) { Vector3D out = null; out = mBezierMultiplyV0_V3[0].multiply(u * u * u); out = out.add(mBezierMultiplyV0_V3[1].multiply(u * u)); out = out.add(mBezierMultiplyV0_V3[2].multiply(u)); out = out.add(mBezierMultiplyV0_V3[3]); return out; } private void advanceMissile() { float u = timeFromLaunch / expectedFlyTime; Vector3D newLocation = this.getCurrentCurveLocation(u); this.location = newLocation; //calc the change of angle this.dirForward = newLocation.minus(this.prevLocation).getNormalized(); this.dirUp = ((this.dirForward).crossProduct(this.dirLeft)).getNormalized(); //cross-product of left and forward is the new up direction this.prevLocation = newLocation; }

Get ready for impact!

I’ve talked about what happens when a missile hits, and about the missile’s course, but how do we know when the missile hits the ground?
We have implemented this using a KD-tree over all the vertices in the world mesh, to check what is the closest vertex to the missile. When the missile gets close enough we check the dot product of the missile’s location and the normal of the surface – when the sign flips the missile hit the ground.

protected void checkForHitWithMesh() { Vector3D tipLocation = location.add(dirForward.multiply(distToTip)); double[] key = {tipLocation.x, tipLocation.y, tipLocation.z}; //check with KD Tree to get the nearest vertex Vertex closestVetrex = null; try { closestVetrex = VertexKdTree.getVertexKdTree().nearest(key); } catch (KeySizeException e) { e.printStackTrace(); return; } //check if missile is lost in space... Vector3D closestVertLocationWorld = aimer.getTransformToMeshLocation().transform(closestVetrex.getVector3D()); if (tipLocation.minus(closestVertLocationWorld).getNorm() > 1) { projectileHandler.setCurrentProjectile(null); flightFinished = true; objNModeHandler.nextMode(); return; } //Check if missile is inside ground if ((closestVetrex.getNormal()).innerProduct(tipLocation.minus(closestVertLocationWorld)) < 0) { flightFinished = true; hitMeshAtVertex(closestVetrex); projectileHandler.setCurrentProjectile(null); } }

Particle Shmarticle

Particle systems are a decent way to simulate smoke, fire, water splashes and anything “particley”. We ripped some C code from the net (I can’t remember where, but it was GPLed), again, and ported it to Java.
The only problem is that OpenGL can’t really display particles, you need something that has some kind of surface, or a line. Other options are drawing GL_POINTs, or GLU spheres, but both options either don’t look pleasing or are very costly in terms of performance. So we used GL_TRIANGLE_STRIP to draw small rectangles of random sizes as the particles.

public void drawParticles(GL gl) { gl.glEnable(GL.GL_BLEND); gl.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA); gl.glDisable(GL.GL_LIGHTING); for (int i = 0; i < MAX_PARTICLES; i++) { // Each particle is handled differently depending on whether it's // alive or not. Particle particle = m_aParticles[i]; if (particle.isAlive()) { // This particular particle is alive. handleLiveParticle(gl, particle); } else { // This particular particle is dead. handleDeadParticle(gl, particle); } } gl.glDisable(GL.GL_BLEND); gl.glEnable(GL.GL_LIGHTING); } private void handleLiveParticle(GL gl, Particle particle) { // The current location of the particle; Need to account for the // zoom // distance so user can zoom in and out the particles. float x = particle.getXLocation(); float y = particle.getYLocation(); float z = particle.getZLocation(); // Set the color to draw this particle. The particle's life value // will act as the alpha. gl.glColor4f(particle.getRed(), particle.getGreen(), particle.getBlue(), particle.getLife()); // Draw the particle using triangle strips. gl.glBegin(GL.GL_TRIANGLE_STRIP); // Map the texture and create the vertices for the particle. float pSize = (float) Math.random() * particle.getLife(); float r = (float) Math.random() - pSize; gl.glVertex3f(x + pSize, y + pSize, z + r); gl.glVertex3f(x - pSize, y + pSize, z + r); gl.glVertex3f(x + pSize, y - pSize, z - r); gl.glVertex3f(x - pSize, y - pSize, z - r); gl.glEnd(); // Update the particles' properties. updateParticle(particle); }

Note that we make sure the “particles” are blended into the buffer, as particles’ color is fading as they near death.

Sum up

OK, I’ve tried to share most of the interesting points in the making of the game. The code is downloadable from the Google code SVN repo. And here’s a short video explaining some aspects of playing the game:

Thanks for tuning in!
Roy.

Augmented reality on the iPhone using NyARToolkit [w/ code]

Roy — Wed, 01 Jul 2009 11:37:26 +0000

Hi

I saw the stats for the blog a while ago and it seems that the augmented reality topic is hot! 400 clicks/day, that’s awesome!

So I wanted to share with you my latest development in this field – cross compiling the AR app to the iPhone. A job that proved easier than I originally thought, although it took a while to get it working smoothly.

Basically all I did was take NyARToolkit, compile it for armv6 arch, combine it with Norio Namura’s iPhone camera video feed code, slap on some simple OpenGL ES rendering, and bam – Augmented Reality on the iPhone.

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

This is how I did it…

I recommend you read my last post on this matter. I have some insights, however superficial, to working with NyARToolkit implementation for C++, that I also use here.

Getting NyARToolkit C++ to compile on iPhone

First of all, I needed to cross-compile NyARToolkit for iPhone’s CPU architecture (Arm), but this was a very simple task – it just compiled off the bat! No tweaking done, what so ever.
But that’s only the beginning, as iPhone apps are built using Objective-C and not C++ (maybe they can, but all the documentation is in obj-c). So I needed to write an Obj-C wrapper around NyARTk to allow my iPhone app to interact with it.

I only needed a very small set of functions out of NyARTk to get Aug.Reality – those that have to do with marker detection. I ended up with a lean API:

@interface NyARToolkitWrapper : NSObject { bool wasInit; } -(void)initNyARTwithWidth:(int)width andHeight:(int)height; -(bool)detectMarker:(float[])resultMat; -(void)setNyARTBuffer:(Byte*)buf; -(void)getProjectionMatrix:(float[])m;

I also have some functions I used for debugging, and non-optimized stages. The inner works of the wrapper are not very interesting (and you can see them in the code yourself), they are mainly invoking NyARSingleDetectMarker functions.

In the beginning – there was only marker detection

OK, to get AR basically what I need to do is:

initialize NyARTk inner structs

set NyARTk’s RGBA buffer with each frame’s pixles

get the extrinsic parameters of the camera, and draw the OpenGL scene accordingly

This is for full fledged AR, but let me start with a simpler case – detecting the market in a single image read from a file. No OpenGL, no camera. Just reading the file’s pixels data, and feeding it to NyARTk.

Now this is far more simple:

CGImageRef img = [[UIImage imageNamed:@"test_marker.png"] CGImage]; int width = CGImageGetWidth(img); int height = CGImageGetHeight(img); Byte* brushData = (Byte *) malloc(width * height * 4); CGContextRef cgctx = CGBitmapContextCreate(brushData, width, height, 8, width * 4, CGImageGetColorSpace(img), kCGImageAlphaPremultipliedLast); CGContextDrawImage(cgctx, CGRectMake(0, 0, (CGFloat)width, (CGFloat)height), img); CGContextRelease(cgctx); [nyartwrapper initNyARTwithWidth:width andHeight:height]; [nyartwrapper setNyARTBuffer:brushData]; [nyartwrapper detectMarker:ogl_camera_matrix];

First I read the image to UIImage, then get it’s respective CGImage. But what I need are bytes, so I create a temporary CGBitmapContext, draw the image into it and use the context pixel data (allocated by me).

Adding the 3D rendering

This is nice, but nothing is shown to the screen, which sux. So the next step will be to create an OpenGL scene, and draw some 3D using the calibration we now have. To do this I used EAGLView from Apple’s OpenGL ES docs.
This view will setup an environment to draw a 3D scene, by giving you a delegate to do the actual drawing while hiding all the perepherial code (frame buffers… and other creatures you wouldn’t want to meet in a dark 3D alley scene).

All I needed to implement in my code were two functions defined in the protocol:

@protocol _DGraphicsViewDelegate @required // Draw with OpenGL ES -(void)drawView:(_DGraphicsView*)view; @optional -(void)setupView:(_DGraphicsView*)view; @end

‘setupView’ will initialize the scene, and ‘drawView’ will draw each frame. In setupView we’ll have the viewport setting, lighting, generating texture buffers etc., You can see all that in the code, it’s not very interesting…

In drawView we’ll draw the background and the 3D scene. Now this took some trickery. First I though i’ll take the easy route and just have the 3D scene be transparent, draw the view using a simple UIView of some kind, and overlay the 3D over it. I didn’t manage to get it to work, so I took a different path (harder? don’t know) and I decided to paint the background over a 3D plane, in the 3D scene itself, using textures. This is how I did it in all my AR app on other devices.
Now, the camera video feed is 304×400 pixels, and OpenGL textures are best optimized at power-of-2 sizes, so I created a 512×512 texture. But for now we’re talking about a single frame.

const GLfloat spriteTexcoords[] = {0,0.625f, 0.46f,0.625f, 0,0, 0.46f,0,}; const GLfloat spriteVertices[] = {0,0,0, 1,0,0, 0,1,0 ,1,1,0}; glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrthof(0, 1, 0, 1, -1000, 1); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); // Sets up pointers and enables states needed for using vertex arrays and textures glEnableClientState(GL_VERTEX_ARRAY); glVertexPointer(3, GL_FLOAT, 0, spriteVertices); glEnableClientState(GL_TEXTURE_COORD_ARRAY); glTexCoordPointer(2, GL_FLOAT, 0, spriteTexcoords); glBindTexture(GL_TEXTURE_2D, spriteTexture); glEnable(GL_TEXTURE_2D); glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_TEXTURE_COORD_ARRAY); glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); glPopMatrix();

Basically, I go into orthographic mode and draw a rectangle with the texture on it, nothing fancy.

Next up – drawing the perspective part of the scene, the part that aligns with the actual camera…

//Load the projection matrix (intrinsic parameters) glMatrixMode(GL_PROJECTION); glLoadMatrixf(ogl_projection_matrix); //Load the "camera" matrix (extrinsic parameters) glMatrixMode(GL_MODELVIEW); glLoadMatrixf(ogl_camera_matrix); glLightfv(GL_LIGHT0, GL_POSITION, lightPosition); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glDisable(GL_TEXTURE_2D); glDisableClientState(GL_TEXTURE_COORD_ARRAY); glPushMatrix(); glScalef(kTeapotScale, kTeapotScale, kTeapotScale); { static GLfloat spinZ = 0.0; glRotatef(spinZ, 0.0, 0.0, 1.0); glRotatef(90.0, 1.0, 0.0, 0.0); spinZ += 1.0; } glEnableClientState(GL_VERTEX_ARRAY); glEnableClientState(GL_NORMAL_ARRAY); glVertexPointer(3 ,GL_FLOAT, 0, teapot_vertices); glNormalPointer(GL_FLOAT, 0, teapot_normals); glEnable(GL_NORMALIZE); for(int i = 0; i < num_teapot_indices; i += new_teapot_indicies[i] + 1) { glDrawElements(GL_TRIANGLE_STRIP, new_teapot_indicies[i], GL_UNSIGNED_SHORT, &new_teapot_indicies[i+1]); } glPopMatrix();

For this also I learned from Apple’s OpenGL ES docs (find it here). I ended up with this:

Tying it together with the camera

This runs on the simulator, since the camera is not involved just yet. I used it to fix the lighting and such, before moving to the device. But we’re here to get it work on the device, so next I plugged in the code from Norio Nomura.
Some people have asked me to post up a working version of Nomura’s code, so you can get it with the code for this app (scroll down). Nomura was kind enough to make it public under MIT license.

First, I set up a timer to fire in ~11fps, and initialize the camera hook to grab the frames from the internal buffers:

repeatingTimer = [NSTimer scheduledTimerWithTimeInterval:0.0909 target:self selector:@selector(load2DTexFromFile:) userInfo:nil repeats:YES]; ctad = [[CameraTestAppDelegate alloc] init]; [ctad doInit];

And then I take the pixel data and use it for the background texture and the marker detection:

-(void)load2DTexWithBytes:(NSTimer*) timer { if([ctad getPixelData] != NULL) { CGSize s = [ctad getVideoSize]; glBindTexture(GL_TEXTURE_2D, spriteTexture); glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, s.width, s.height, GL_BGRA, GL_UNSIGNED_BYTE, [ctad getPixelData]); if(![nyartwrapper wasInit]) { [nyartwrapper initNyARTwithWidth:s.width andHeight:s.height]; [nyartwrapper getProjectionMatrix:ogl_projection_matrix]; [nyartwrapper setNyARTBuffer:[ctad getPixelData]]; } [nyartwrapper detectMarker:ogl_camera_matrix]; } }

All this happens 11 times per second, so it must be concise.

Video proof time…

Well, looks like we are pretty much done! time for a video…

How did you get the phone to stand still so nicely?

An important issue… when it comes to shooting the phone w/o holding it.
Well I used a little piece of metal that’s used to block the PCI docks in the PC. In hebrew will call these scrap metal “Flakch”s (don’t try to pronounce this at home). I bended it in the middle to create a kind of “leg”, and the ledge to hold the phone already exists.

The code

As promised, here’s the code (I omitted some files whose license is questionable).

That’s all folks!
See you when I get this to work on the Android…
Roy.