OpenKinect and Qt reference Design

UPDATE (you can now get a new version of the code here and I'm in the process of updating this to work with the new drivers)

After seeing all the cool projects here I decided to buy a Kinect and have a play. Most of the code is really simple and the library itself includes a couple of sample apps. I  decided as Qt was my main development environment to wrap the whole library into a Qt ready class so I could use the signals and slots mechanism as well as Qt's Threading capabilities.

Getting Started

First thing to do is download and install the libfreenect source using git from here this installs to /usr/local so is ready to be linked in. To test try out the sample programs. If you are using a mac (as I am) you will need to patch the libusb source but full instructions are on the site.

Integrating into Qt
When using Qt objects it is possible to use the signals and slots mechanism to pass messages to each of the objects, this is useful as we can connect GUI components to a class and use the event mechanism to for example change the angle of the Kinect.

To do this we need to inherit from the base QObject class and extend our class with the signals and slots required. Luckily the slots are also normal methods so we can use our class in two ways.

As we are wrapping into an existing C library there are a number of things to consider. Firstly the libfreenect library uses callbacks to grab the buffer data, we really need to make this work in a separate thread so that it can work at it's own pace, we also need to be able to grab the frame data when it's not being accessed by the kinect library, therefore we need to add a Mutex to lock our data whilst the thread is using it.

As Qt has it's own built in Thread, Mutex and MutexLocker classes I decided to use these to make life easier.

Finally we only need one instance of the class, so that we can load the callbacks once then grab the data, the easiest way of doing this is to use the singleton pattern and each class that needs access to the QKinect object can grab the instance required.

The basic class outline for the QKinect object is as follows

Object Construction

As mentioned above the class is based on the singleton pattern, however I ran into a problem with my initial design, and it's worth sharing that problem here.

Usually In the singleton pattern we have an instance method which will call the constructor of the class and initialise elements of that class. When I did this I was getting issues and with some debug statements I found out that the ctor was being called twice but I was getting a null instance each time. Finally I realised this was due to the fact that the constructor was creating two threaded callbacks which called the instance method, however the instance pointer had not yet been created. To overcome this problem the following instance method was created.

QKinect* QKinect::instance()
{
 // this is the main singleton code first check to see if we exist
 if (s_instance==0 )
 {
  // we do so create an instance (this will validate the pointer so other
  // methods called in the init function will have a valid pointer to use)
  s_instance = new QKinect;
  // now setup the actual class (with a valid pointer)
  /// \note this could be made nicer to make it fully thread safe
  s_instance->init();
 }
 // otherwise return the existing pointer
  return s_instance;
}

The constructor does nothing but return an instance of the class to the s_instance pointer. After this we call the init method on this class which will initialise the device and start the threading.

void QKinect::init()
{
 // first see if we can init the kinect
 if (freenect_init(&m_ctx, NULL) < 0)
 {
  qDebug()<<"freenect_init() failed\n";
  exit(EXIT_FAILURE);
 }
 /// set loggin level make this programmable at some stage
 freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);
 /// see how many devices we have
 int nr_devices = freenect_num_devices (m_ctx);
 /// now allocate the buffers so we can fill them
 m_userDeviceNumber = 0;
 m_bufferDepth.resize(FREENECT_VIDEO_RGB_SIZE);
 m_bufferVideo.resize(FREENECT_VIDEO_RGB_SIZE);
 m_bufferDepthRaw.resize(FREENECT_FRAME_PIX);
 m_bufferDepthRaw16.resize(FREENECT_FRAME_PIX);
 m_gamma.resize(2048);
 /// open the device at present hard coded to device 0 as I only
 /// have 1 kinect
 if (freenect_open_device(m_ctx, &m_dev, m_userDeviceNumber) < 0)
 {
  qDebug()<<"Could not open device\n";
  exit(EXIT_FAILURE);
 }


 /// build the gamma table used for the depth to rgb conversion
 /// taken from the demo programs
 for (int i=0; i<2048; ++i)
 {
  float v = i/2048.0;
  v = std::pow(v, 3)* 6;
  m_gamma[i] = v*6*256;
 }
 /// init our flags
 m_newRgbFrame=false;
 m_newDepthFrame=false;
 m_deviceActive=true;
 // set our video formats to RGB by default
 /// \todo make this more flexible at some stage
 freenect_set_video_format(m_dev, FREENECT_VIDEO_RGB);
 freenect_set_depth_format(m_dev, FREENECT_DEPTH_11BIT);
 /// hook in the callbacks
 freenect_set_depth_callback(m_dev, depthCallback);
 freenect_set_video_callback(m_dev, videoCallback);
 // start the video and depth sub systems
 startVideo();
 startDepth();
 // set the thread to be active and start
 m_process = new QKinectProcessEvents(m_ctx);
 m_process->setActive();
 m_process->start();
}

Most if this code is following some of the examples from the freenect library but using the class attributes to store the data. We also create our main processing thread to loop and process the Kinect events

QThread

The QThread object is a class which we inherit from to allow crosss platform threading, for this class we must implement a run() method which is called when the QThread start method is called. The class is as follows

class QKinectProcessEvents : public QThread
{
public :
 /// @brief ctor where we pass in the context of the kinect
 /// @param [in] _ctx the context of the current kinect device
 inline QKinectProcessEvents(
               freenect_context *_ctx
               )
               {m_ctx=_ctx;}
 /// @brief sets the thread active this will loop the run thread
 /// with a while(m_active) setting this will end the thread loop
 inline void setActive(){m_active=true;}
 /// @brief sets the thread active must call QThread::start again to make this
 /// work if the thread has been de-activated
 inline void setInActive(){m_active=false;}
protected :
 /// @brief the actual thread main loop, this is not callable and the
 /// QThread::start method of QThread must be called to activate the loop
 void run();

private :
 /// @brief a flag to indicate if the loop is to be active
 /// set true in the ctor
 bool m_active;
 /// @brief the context of the kinect device, this must
 /// be set before the thread is run with QThread::start
 freenect_context *m_ctx;
};

The run method itself is quite simple, it will loop whilst the m_active flag is true (set in the init method above)

void QKinectProcessEvents::run()
{
 // loop while we are active and process the kinect event queue
 while(m_active)
 {
  //qDebug()<<"process thread\n";
  if(freenect_process_events(m_ctx) < 0)
  {
   throw std::runtime_error("Cannot process freenect events");
  }
 }
}

How libfreenect works
The freenect library requires callbacks to connect to the device which are passed the active device as well as a void** pointer into which the data will be loaded. As it is a C library it requires static methods to be passed and the signature of the methods must conform with the callback methods from the library. The following code shows how these are implemented.

static inline void depthCallback(
                                 freenect_device *_dev, 
                                 void *_depth,
                                 uint32_t _timestamp=0
                                )
 {
  /// get an instance of our device
  QKinect *kinect=QKinect::instance();
  /// then call the grab method to fill the depth buffer and return it
  kinect->grabDepth(_depth,_timestamp);
 }
 
 static inline void videoCallback(
                                         freenect_device *_dev,
                                         void *_video,
                                         uint32_t _timestamp=0
                                        )
 {
  /// get an instance of our device
  QKinect *kinect=QKinect::instance();
  /// then fill the video buffer
  kinect->grabVideo(_video, _timestamp);
 }

These methods grab an instance of the class so we can access the class methods, then we call the grab methods to actually fill in the buffers. The simplest of these is the grabVideo method as shown below

void QKinect::grabVideo(
                         void *_video,
                         uint32_t _timestamp
                       )
{
 // lock our mutex and copy the data from the video buffer
 QMutexLocker locker( &m_mutex );
 uint8_t* rgb = static_cast(_video);
 std::copy(rgb, rgb+FREENECT_VIDEO_RGB_SIZE, m_bufferVideo.begin());
 m_newRgbFrame = true;
}

This method is made thread safe by using the QMutexLocker, this object is passed a mutex which it locks, when the object falls out of scope it automatically unlocks the object when it is destroyed. This makes life a lot easier as we don't have to write an unlock for each exit point of the method. The data is passed to the *_video pointer from libfreenect and we then just copy it to our buffer ready for the other subsystems to use.

The depth callback is more complex as I grab the data in different formats, the easiest two are the method that just fill the buffers with the raw 16 bit data and also the raw data compressed as an 8 bit (this is not quite correct and is likely to go soon). There is also code from the sample apps to convert this into an RGB gamma corrected data buffer, this gives red objects closest to the camera then green then blue for distance. This is useful for visualising the depth data.

void QKinect::grabDepth(
                         void *_depth,
                         uint32_t _timestamp
                       )
{
// this method fills all the different depth buffers at once
// modifed from the sample code glview and cppview.cpp
/// lock our mutex
QMutexLocker locker( &m_mutex );
// cast the void pointer to the unint16_t the data is actually in
uint16_t* depth = static_cast(_depth);

// now loop and fill data buffers
for( unsigned int i = 0 ; i < FREENECT_FRAME_PIX ; ++i)
{
 // first our two raw buffers the first will lose precision and may well
 // be removed in the next iterations
 m_bufferDepthRaw[i]=depth[i];
 m_bufferDepthRaw16[i]=depth[i];
 // now get the index into the gamma table
 int pval = m_gamma[depth[i]];
 // get the lower bit
 int lb = pval & 0xff;
 // shift right by 8 and determine which colour value to fill the
 // array with based on the position
 switch (pval>>8)
 {
 case 0:
  m_bufferDepth[3*i+0] = 255;
  m_bufferDepth[3*i+1] = 255-lb;
  m_bufferDepth[3*i+2] = 255-lb;
  break;
 case 1:
  m_bufferDepth[3*i+0] = 255;
  m_bufferDepth[3*i+1] = lb;
  m_bufferDepth[3*i+2] = 0;
  break;
 case 2:
  m_bufferDepth[3*i+0] = 255-lb;
  m_bufferDepth[3*i+1] = 255;
  m_bufferDepth[3*i+2] = 0;
  break;
 case 3:
  m_bufferDepth[3*i+0] = 0;
  m_bufferDepth[3*i+1] = 255;
  m_bufferDepth[3*i+2] = lb;
  break;
 case 4:
  m_bufferDepth[3*i+0] = 0;
  m_bufferDepth[3*i+1] = 255-lb;
  m_bufferDepth[3*i+2] = 255;
  break;
 case 5:
  m_bufferDepth[3*i+0] = 0;
  m_bufferDepth[3*i+1] = 0;
  m_bufferDepth[3*i+2] = 255-lb;
  break;
 default:
  m_bufferDepth[3*i+0] = 0;
  m_bufferDepth[3*i+1] = 0;
  m_bufferDepth[3*i+2] = 0;
  break;
 }
}
// flag we have a new frame
m_newDepthFrame = true;
}

Using the class
Now the class has been created it quite simple to use. First we need to create an instance of the class as follows

QKinect *m_kinect;

m_kinect=QKinect::instance();

// now connect a QSpinBox to the angle

QDoubleSpinBox *angle = new QDoubleSpinBox(this);
angle->setMaximum(30.0);
angle->setMinimum(-30.0);
angle->setSingleStep(1.0);
QObject::connect(angle,SIGNAL(valueChanged(double)),m_kinect,SLOT(setAngle(double)));

Drawing the image buffer
The easiest way to draw the data from one of the image buffers is to use an OpenGL texture buffer and attach it to a Quad. There are many ways to do this and usually I will use retained mode OpenGL and a series of shaders to make things work faster on the GPU, however I wanted this system not to require too many external libraries (such as my ngl:: lib which I use for other examples) so I've used a simple immediate mode GL version for this example.

// first create the gl texture (called once)
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_TEXTURE_2D);
glGenTextures(1, & m_rgbTexture);
glBindTexture(GL_TEXTURE_2D, m_rgbTexture);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);


// bind data and draw (per frame)
QKinect *kinect=QKinect::instance();
if(m_mode ==0)
{
 kinect->getRGB(m_rgb);
}
else if(m_mode == 1)
{
 kinect->getDepth(m_rgb);
}
glBindTexture(GL_TEXTURE_2D, m_rgbTexture);
glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, &m_rgb[0]);
glLoadIdentity();

glEnable(GL_TEXTURE_2D);

glBegin(GL_TRIANGLE_FAN);
 glColor4f(255.0f, 255.0f, 255.0f, 255.0f);
 glTexCoord2f(0, 0); glVertex3f(0,0,0);
 glTexCoord2f(1, 0); glVertex3f(640,0,0);
 glTexCoord2f(1, 1); glVertex3f(640,480,0);
 glTexCoord2f(0, 1); glVertex3f(0,480,0);
glEnd();

The following movie show the reference implementation in action

You can grab the source code from my website here t