Wednesday, 22 December 2010

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
video

You can grab the source code from my website here t

14 comments:

  1. Great work. This is exactly what I was looking for. I am relatively new to C++ and am starting to work into QT. Thank you for taking the time to document everything. It was a great read and I learned quite abit about programming in general. I will be referencing your work quite often while I work on my project.

    ReplyDelete
  2. No worries Chris, would be good to see what you come up with, please keep me updated.

    ReplyDelete
  3. Hi Jon, I had the same idea about trying out kinect and just got hold of one myself today. While looking into it on the net I came across your article, Looked at the video and thought you looked rather familiar. It's Dan Shepherd (previous NCCA person). I've been looking into OpenNI and NITE more than freenect, but NITE won't compile on my old computer as it requires a better processor with SSE3 enabled instruction set I think?

    ReplyDelete
  4. PS. Nice article, Until I tried out your sample I didn't realize it was possible to control the LED's or that the kinect had a servo motor in it, not that these points are relevant the your tutorial. All they best, Dan

    ReplyDelete
  5. Hi Dan, good to hear from you, have fun with the code

    ReplyDelete
  6. best thing i found so far thank you very very very much!

    ReplyDelete
  7. Hi, when I run the project show "Cannot start RGB callback".

    Any idea?

    ReplyDelete
  8. Not sure but that sounds like a problem with the OpenKinect Drivers as that message is not part of my code.

    ReplyDelete
  9. Hi Jon,

    Its Sundar, your student in current MSc batch.
    Even I got the same error "Cannot start RGB callback". Later I found, the functions freenect_set_video_format() and freenect_set_depth_format() in the libfreenect header have been replaced with freenect_set_video_mode() and freenect_set_depth_mode() respectively.

    Replacing them in QKinect::init() with suitable arguments, I got it working!

    Many Thanks.

    ReplyDelete
  10. Hi Sun Ray,

    could you post the working code? :) Tnx.

    ReplyDelete
  11. Hello Samo,

    Sorry for the late reply.

    The working code is

    freenect_set_depth_callback(m_dev, depthCallback);
    freenect_set_video_callback(m_dev, videoCallback);
    freenect_set_video_mode(m_dev,freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
    freenect_set_depth_mode(m_dev,freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_DEPTH_11BIT));

    ReplyDelete
  12. Hi everyone,

    Im trying to use this code to obtain images from kinect but i have been struggling with the same error for 2 days and cant find a solution.

    Thanks in advance for any help...

    ERROR:

    Undefined symbols:
    "_freenect_set_log_level", referenced from:
    QKinect::init() in QKinect.o
    "vtable for QKinect", referenced from:
    __ZTV7QKinect$non_lazy_ptr in QKinect.o
    (maybe you meant: __ZTV7QKinect$non_lazy_ptr)
    .
    .
    .
    .
    .
    "_freenect_start_video", referenced from:
    QKinect::startVideo() in QKinect.o
    QKinect::setVideoMode(int) in QKinect.o
    ld: symbol(s) not found
    collect2: ld returned 1 exit status

    I have some basic knowledge on C++... but not so much to see clearly what is happening.

    Thanks again.

    ReplyDelete
  13. Hi Jon,

    I have installed libfreenect on Win7 to work with mingw, run test app glview.exe, everything works fine. But when I compile your source code, i have following errors. I googled the error, it is written there that it occures when i use not-stable version. But the issue is, i have installed master version (stable).

    Any ideas?

    Thank you in advance, and for the great post!

    Error list:

    ../QTReferenceProject/src/QKinect.cpp: In member function 'void QKinect::init()':
    ../QTReferenceProject/src/QKinect.cpp:112: error: 'FREENECT_VIDEO_RGB_SIZE' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp:114: error: 'FREENECT_FRAME_PIX' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp:141: error: 'freenect_set_video_format' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp:142: error: 'freenect_set_depth_format' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp: In member function 'void QKinect::setYellowLedFlash()':
    ../QTReferenceProject/src/QKinect.cpp:201: error: 'LED_BLINK_YELLOW' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp: In member function 'void QKinect::setVideoMode(int)':
    ../QTReferenceProject/src/QKinect.cpp:229: error: 'freenect_set_video_format' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp: In member function 'void QKinect::grabDepth(void*, uint32_t)':
    ../QTReferenceProject/src/QKinect.cpp:351: error: 'FREENECT_FRAME_PIX' was not declared in this scope
    ../QTReferenceProject/src/QKinect.cpp: In member function 'void QKinect::grabVideo(void*, uint32_t)':
    ../QTReferenceProject/src/QKinect.cpp:414: error: 'FREENECT_VIDEO_RGB_SIZE' was not declared in this scope

    ReplyDelete
  14. you may need to use the updated version of the code from here http://nccastaff.bournemouth.ac.uk/jmacey/GraphicsLib/Demos/index.html (it's at the bottom of the page)

    ReplyDelete