Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

KinectDriver.cc

Go to the documentation of this file.
00001 #ifdef HAVE_OPENNI
00002 
00003 #include <stdint.h>
00004 #include "KinectDriver.h"
00005 
00006 /****************
00007 * Kinect Driver *
00008 ****************/
00009 
00010 #define CHECK_RC(rc, what) \
00011   if (rc != XN_STATUS_OK) { \
00012     cerr << "KinectDriver: " << what << " failed " << xnGetStatusString(rc) << endl; \
00013   }
00014 
00015 #define CHECK_ERRORS(rc, errors, what) \
00016   if (rc == XN_STATUS_NO_NODE_PRESENT) { \
00017     XnChar strError[1024]; \
00018     errors.ToString(strError, 1024); \
00019     cerr << "KinectDriver: " << strError << endl; \
00020   }
00021 
00022 const string KinectDriver::autoRegisterKinectDriver =
00023   DeviceDriver::getRegistry().registerType<KinectDriver>("Kinect");
00024 
00025 KinectDriver::KinectDriver(const string& name) :
00026   DeviceDriver(autoRegisterKinectDriver,name),
00027   context(), scriptNode(), image(), depth(), rgbSource(this), depthSource(this) {
00028   // Load the OpenNI configuration file, which has the sensors available
00029   stringstream ss;
00030   ss << "ms/config/OpenNI-" << name << ".xml";
00031   xn::EnumerationErrors errors;
00032   XnStatus rc = context.InitFromXmlFile(ss.str().c_str(), scriptNode, &errors);
00033   CHECK_ERRORS(rc, errors, "InitFromXmlFile");
00034   CHECK_RC(rc, "InitFromXmlFile");
00035 
00036   // Fetch the image and depth generators
00037   context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
00038   context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
00039 
00040   // Start getting data
00041   context.StartGeneratingAll();
00042 }
00043 
00044 KinectDriver::~KinectDriver() { context.Release(); }
00045 
00046 /*************
00047 * RGB Source *
00048 *************/
00049 
00050 const string KinectDriver::RGBSource::instanceName = "RGBSource";
00051 
00052 void KinectDriver::RGBSource::imageUpdateReceived(ImageGenerator &image, RGBSource *source) {
00053   // cout << instanceName << ": update received" << endl;
00054   source->advance();
00055 }
00056 
00057 bool KinectDriver::RGBSource::advance()
00058 {
00059   driver->image.GetMetaData(metadata);
00060   // cout << instanceName << ": advance " << (unsigned int)metadata.FrameID() << " " << (long long)metadata.Timestamp() << endl;
00061 
00062   int width = metadata.XRes();
00063   int height = metadata.YRes();
00064   int components = 3;
00065 
00066   RCRegion* region = getUnusedRegion(width*height*components+sizeof(ImageHeader), 0);
00067   uint8_t * buf = reinterpret_cast<uint8_t*>(region->Base());
00068   new (buf) ImageHeader(ProjectInterface::visRawCameraSID, 0, width, height, components, frameNumber++, metadata.Timestamp(), nextName());
00069   uint8_t * yuv = buf+sizeof(ImageHeader);
00070 
00071   if (driver->image.GetPixelFormat() == XN_PIXEL_FORMAT_RGB24) {
00072     const XnRGB24Pixel *rgb = driver->image.GetRGB24ImageMap();
00073     for (int i=0; i<height*width; i++) {
00074       float R = rgb[i].nRed;
00075       float G = rgb[i].nGreen;
00076       float B = rgb[i].nBlue;
00077       *yuv++ = (0.257 * R) + (0.504 * G) + (0.098 * B) + 16;
00078       *yuv++ = -(0.148 * R) - (0.291 * G) + (0.439 * B) + 128;
00079       *yuv++ = (0.439 * R) - (0.368 * G) - (0.071 * B) + 128;
00080     }
00081   } else if (driver->image.GetPixelFormat() == XN_PIXEL_FORMAT_YUV422) {
00082     const XnYUV422DoublePixel *uyvy = driver->image.GetYUV422ImageMap();
00083     for (int i=0; i<height*width; i+=2) {
00084       *yuv++ = uyvy[i].nY1;
00085       *yuv++ = uyvy[i].nU;
00086       *yuv++ = uyvy[i].nV;
00087       *yuv++ = uyvy[i+1].nY2;
00088       *yuv++ = uyvy[i+1].nU;
00089       *yuv++ = uyvy[i+1].nV;
00090     }
00091   }
00092 
00093   setImage(region);
00094   return true;
00095 }
00096 
00097 void KinectDriver::RGBSource::doUnfreeze() {
00098   // listen for more data
00099   if ( driver->image.IsValid() )
00100     driver->image.RegisterToNewDataAvailable((StateChangedHandler)&imageUpdateReceived, this, cbHandle);
00101 }
00102 
00103 void KinectDriver::RGBSource::doFreeze() {
00104   // don't listen for more data
00105   if ( driver->image.IsValid() )
00106     driver->image.UnregisterFromNewDataAvailable(cbHandle);
00107 }
00108 
00109 /***************
00110 * Depth Source *
00111 ***************/
00112 
00113 const string KinectDriver::DepthSource::instanceName = "DepthSource";
00114 
00115 void KinectDriver::DepthSource::depthUpdateReceived(DepthGenerator &depth, DepthSource *source) {
00116   // cout << instanceName << ": update received" << endl;
00117   source->advance();
00118 }
00119 
00120 bool KinectDriver::DepthSource::advance()
00121 {
00122   driver->depth.GetMetaData(metadata);
00123   // cout << instanceName << ": advance " << (unsigned int)metadata.FrameID() << " " << (long long)metadata.Timestamp() << endl;
00124 
00125   int width = metadata.XRes();
00126   int height = metadata.YRes();
00127   int components = 2; // 2 byte short integer per pixel
00128 
00129   RCRegion* region = getUnusedRegion(width*height*components+sizeof(ImageHeader), 0);
00130   uint8_t * buf = reinterpret_cast<uint8_t*>(region->Base());
00131   new (buf) ImageHeader(ProjectInterface::visRawDepthSID, 0, width, height, components, frameNumber++, metadata.Timestamp(), nextName());
00132   uint16_t * rawmap = (uint16_t*)(buf+sizeof(ImageHeader));
00133 
00134   //  const XnDepthPixel *map = depth.GetDepthMap();
00135   const XnDepthPixel *map = metadata.Data();
00136   for (int i=0; i<height*width; i++) {
00137     *rawmap++ = map[i];
00138   }
00139   
00140   setImage(region);
00141   return true;
00142 }
00143 
00144 void KinectDriver::DepthSource::doUnfreeze() {
00145   // listen for more data
00146   if ( driver->depth.IsValid() )
00147     driver->depth.RegisterToNewDataAvailable((StateChangedHandler)&depthUpdateReceived, this, cbHandle);
00148 }
00149 
00150 void KinectDriver::DepthSource::doFreeze() {
00151   // don't listen for more data
00152   if ( driver->depth.IsValid() )
00153     driver->depth.UnregisterFromNewDataAvailable(cbHandle);
00154 }
00155 
00156 #endif /* HAVE_OPENNI */

Tekkotsu Hardware Abstraction Layer 5.1CVS
Generated Mon May 9 05:01:38 2016 by Doxygen 1.6.3