Kinect + Irrlicht prototype

main.cpp

#include <irrlicht/irrlicht.h>
#include <libfreenect/libfreenect.h>
#include <iostream>
#include <thread>

using namespace irr;

// global instances and mutexes protecting them
std::mutex g_irr_mutex;
IrrlichtDevice* g_irr;

std::mutex g_f_mutex;
freenect_context* g_f_ctx;
freenect_device* g_f_dev;

// lock for depth back buffer
std::mutex g_backbuf_mutex;
// a notification is sent when the depth buffer is dirty
std::condition_variable g_backbuf_cv;
// depth buffer
uint8_t *g_depth_back;

// global variable indicating an end of execution request
volatile bool g_die = false;

// initializes irrlicht library.
// returns non-zero on failure.
int init_irrlicht()
{
	std::lock_guard<std::mutex> lock(g_irr_mutex);

	g_irr = createDevice(video::EDT_OPENGL, core::dimension2du(640,480), 16, false, false, true, 0);
	g_irr->setWindowCaption(L"Kinect test");

	return g_irr ? 0 : 1;
}

// if the irrlicht library is loaded,
// displays an error message box containing the given message
// otherwise,
// displays the error message in stderr
void disp_error_msg(const wchar_t* msg)
{
	std::lock_guard<std::mutex> lock(g_irr_mutex);

	if (g_irr && g_irr->getGUIEnvironment())
	{
		g_irr->getGUIEnvironment()->addMessageBox(L"Error",msg);
	}
	else
	{
		std::cerr << msg << std::endl;
	}
}

// initializes freenect library.
// returns non-zero on failure.
int init_freenect()
{
	std::lock_guard<std::mutex> lock(g_f_mutex);

	// initialize freenect library
	if (freenect_init(&g_f_ctx, NULL) < 0)
	{
		disp_error_msg(L"freenect_init() failed.");
		return 1;
	}

	// enable debug logging
	freenect_set_log_level(g_f_ctx, FREENECT_LOG_DEBUG);

	// choose to only enable the camera
	freenect_select_subdevices(g_f_ctx, (freenect_device_flags)(FREENECT_DEVICE_CAMERA));

	// make sure we can detect at least 1 device
	if (freenect_num_devices(g_f_ctx) < 1)
	{
		disp_error_msg(L"Did not detect any devices.");
		freenect_shutdown(g_f_ctx);
		return 1;
	}

	// open device
	// TODO: Allow using other device numbers than 0
	if (freenect_open_device(g_f_ctx, &g_f_dev, 0) < 0)
	{
		disp_error_msg(L"Could not open device.");
		freenect_shutdown(g_f_ctx);
		return 1;
	}

	return 0;
}

// main loop for irrlicht
void irr_main()
{
	video::IVideoDriver* driver;
	scene::ISceneManager* smgr;
	gui::IGUIEnvironment* guienv;
	video::ITexture* depth_tex = 0;

	{
	std::lock_guard<std::mutex> lock(g_irr_mutex);

	driver = g_irr->getVideoDriver();
	smgr = g_irr->getSceneManager();
	guienv = g_irr->getGUIEnvironment();

	// Create a texture to allow drawing depth buffer on it
	depth_tex = driver->addTexture(core::dimension2du(640,480), "depth_tex");
	}

	while (!g_die)
	{
		{ // start lock scope
		std::lock_guard<std::mutex> lock(g_irr_mutex);

		if (g_irr->run())
		{
			if (g_irr->isWindowActive())
			{
				{
				// wait for dirty back buffer signal
				std::unique_lock<std::mutex> bbuf_lock(g_backbuf_mutex);
				g_backbuf_cv.wait(bbuf_lock);

				uint32_t* depth_tex_buf = static_cast<uint32_t*>(depth_tex->lock());
				// copy back buffer to texture while they are locked
				for (int i = 0; i < 640*480; ++i)
				{
					depth_tex_buf[i] = (g_depth_back[3*i] << 16 | g_depth_back[3*i+1] << 8 | g_depth_back[3*i+2]);
				}

				depth_tex->unlock();
				}

				driver->beginScene(true,true,video::SColor(255,100,149,237));

				smgr->drawAll();

				if (depth_tex)
				{
					driver->draw2DImage(depth_tex, core::position2di(0,0));
				}
				guienv->drawAll();

				driver->endScene();
			}
			else
			{
				g_irr->yield();
			}
		}
		else
		{
			g_die = true;
		}
		} // end lock scope
	}
}


// callback for depth information received event
void depth_callback(freenect_device* dev, void* v_depth, uint32_t timestamp)
{
	std::lock_guard<std::mutex> lock(g_backbuf_mutex);
	uint16_t* depth = static_cast<uint16_t*>(v_depth);

	for (int i = 0; i < 640*480; ++i)
	{
		float depth_ratio = static_cast<float>(depth[i])/FREENECT_DEPTH_RAW_MAX_VALUE;
		uint8_t depth_color = 255.0*powf(1.0f - depth_ratio, 3);
		g_depth_back[3*i] = depth_color;
		g_depth_back[3*i+1] = depth_color;
		g_depth_back[3*i+2] = depth_color;
	}

	// send signal of depth being dirty
	g_backbuf_cv.notify_all();
}

// main loop for freenect
void freenect_main()
{
	freenect_set_depth_callback(g_f_dev, depth_callback);
	freenect_set_depth_mode(g_f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));

	freenect_start_depth(g_f_dev);

	while (!g_die && freenect_process_events(g_f_ctx) >= 0);

	freenect_stop_depth(g_f_dev);

	freenect_close_device(g_f_dev);
}

int main()
{
	// allocated on the stack to guarantee proper cleanup
	uint8_t depth_back[640*480*3];
	g_depth_back = depth_back;


	if (init_irrlicht())
	{
		disp_error_msg(L"Failed to initialize irrlicht");
		return 1;
	}

	// create thread if freenect initialized successfully
	std::thread* freenect_thread = 0;
	if (!init_freenect())
	{
		freenect_thread = new std::thread(freenect_main);
	}

	// wait until threads finish executing
	// note: seems like irrlicht needs to be in the main thread
	irr_main();
	if (freenect_thread)
	{
		freenect_thread->join();
		delete freenect_thread;
	}
}

SConstruct

env = Environment()

linked_libs = ['Irrlicht', 'GL', 'freenect']
cpp_flags = ['-std=c++0x']

env.Append(LIBS = linked_libs)
if ARGUMENTS.get("debug",0):
	cpp_flags += ['-g']

env.Append(CPPFLAGS = ' '.join(cpp_flags))

env.Program(source=['main.cpp'])