Skip to content

Instantly share code, notes, and snippets.

@nlguillemot
Created May 28, 2012 03:44
Show Gist options
  • Save nlguillemot/2817112 to your computer and use it in GitHub Desktop.
Save nlguillemot/2817112 to your computer and use it in GitHub Desktop.
Kinect + Irrlicht prototype
#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;
}
}
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'])
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment