Fawkes API  Fawkes Development Version
image_thread.cpp
00001 
00002 /***************************************************************************
00003  *  image_thread.cpp - OpenNI image provider thread
00004  *
00005  *  Created: Thu Mar 17 14:06:39 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "image_thread.h"
00024 #include "utils/setup.h"
00025 
00026 #include <core/threading/mutex_locker.h>
00027 #include <fvutils/ipc/shm_image.h>
00028 #include <fvutils/color/colorspaces.h>
00029 #include <fvutils/color/bayer.h>
00030 #include <fvutils/color/yuv.h>
00031 #include <fvutils/color/yuvrgb.h>
00032 
00033 #include <memory>
00034 
00035 using namespace fawkes;
00036 using namespace firevision;
00037 
00038 /** @class OpenNiImageThread "image_thread.h"
00039  * OpenNI Image Provider Thread.
00040  * This thread provides YUV and RGB images from the camera via
00041  * SharedMemoryImageBuffer to other FireVision plugins.
00042  *
00043  * @author Tim Niemueller
00044  */
00045 
00046 /** Constructor. */
00047 OpenNiImageThread::OpenNiImageThread()
00048   : Thread("OpenNiImageThread", Thread::OPMODE_WAITFORWAKEUP),
00049     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
00050 {
00051 }
00052 
00053 
00054 /** Destructor. */
00055 OpenNiImageThread::~OpenNiImageThread()
00056 {
00057 }
00058 
00059 
00060 void
00061 OpenNiImageThread::init()
00062 {
00063   MutexLocker lock(openni.objmutex_ptr());
00064 
00065   __cfg_copy_mode = CONVERT_YUV;
00066 
00067   __image_gen = new xn::ImageGenerator();
00068   std::auto_ptr<xn::ImageGenerator> imagegen_autoptr(__image_gen);
00069 
00070   XnStatus st;
00071 
00072   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_IMAGE, __image_gen);
00073 
00074   fawkes::openni::setup_map_generator(*__image_gen, config);
00075 
00076   __usb_vendor = 0;
00077   __usb_product = 0;
00078 
00079   xn::NodeInfo image_info = __image_gen->GetInfo();
00080   xn::NodeInfoList &depnodes = image_info.GetNeededNodes();
00081   for (xn::NodeInfoList::Iterator n = depnodes.Begin(); n != depnodes.End(); ++n) {
00082     const XnProductionNodeDescription &pnd = (*n).GetDescription();
00083 
00084     if ((pnd.Type == XN_NODE_TYPE_DEVICE) &&
00085         (strcmp(pnd.strVendor, "PrimeSense") == 0) &&
00086         (strcmp(pnd.strName, "SensorV2") == 0) )
00087     {
00088       // it's the primesense device node and we can check for USB vendor/product
00089       unsigned short int vendor = 0, product = 0;
00090       unsigned char bus = 0, addr = 0;
00091       if (sscanf((*n).GetCreationInfo(), "%04hx/%04hx@%hhu/%hhu", &vendor, &product, &bus, &addr) == 4) {
00092         logger->log_debug(name(), "Detected USB device (vendor: %04hx  product: %04hx  bus: %hhu  addr: %hhu)",
00093                           vendor, product, bus, addr);
00094         __usb_vendor  = vendor;
00095         __usb_product = product;
00096       }
00097     }
00098   }
00099 
00100   if ( (__usb_vendor == 0x045e) && (__usb_product == 0x02ae) ) {
00101     // from OpenNI-PrimeSense/XnStreamParams.h:
00102     // XN_IO_IMAGE_FORMAT_UNCOMPRESSED_BAYER = 6
00103     // InputFormat should be 6 = uncompressed Bayer for Kinect
00104     logger->log_debug(name(), "Kinect camera detected, initializing");
00105     if (__image_gen->SetIntProperty("InputFormat", 6) != XN_STATUS_OK) {
00106       throw Exception("Failed to set uncompressed bayer input format");
00107     }
00108     if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT) != XN_STATUS_OK)
00109     {
00110       throw Exception("Failed to set pixel format");
00111     }
00112     /*
00113     // RegistrationType should be 2 (software) for Kinect, 1 (hardware) for PS
00114     // (from ROS openni_camera)
00115     if (__depth_gen->SetIntProperty ("RegistrationType", 2) != XN_STATUS_OK) {
00116       throw Exception("Failed to set registration type");
00117     }
00118     */
00119     __cfg_copy_mode = DEBAYER_BILINEAR;
00120     try {
00121       std::string debayering = config->get_string("/plugins/openni-image/debayering");
00122       if (debayering == "bilinear") {
00123         __cfg_copy_mode = DEBAYER_BILINEAR;
00124       } else if (debayering == "nearest_neighbor") {
00125         __cfg_copy_mode = DEBAYER_NEAREST_NEIGHBOR;
00126       } else {
00127         logger->log_warn(name(), "Unknown de-bayering mode '%s', using bilinear instead.",
00128                          debayering.c_str());
00129       }
00130     } catch (Exception &e) {
00131       logger->log_warn(name(), "No de-bayering mode set, using bilinear.");
00132     }
00133   } else {
00134     logger->log_debug(name(), "PrimeSense camera detected, initializing");
00135     if (__image_gen->SetIntProperty("InputFormat", 5) != XN_STATUS_OK) {
00136       throw Exception("Failed to set uncompressed bayer input format");
00137     }
00138     if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_YUV422) != XN_STATUS_OK) {
00139       throw Exception("Failed to set pixel format");
00140     }
00141     /*
00142     // RegistrationType should be 2 (software) for Kinect, 1 (hardware) for PS
00143     // (from ROS openni_camera)
00144     if (__depth_gen->SetIntProperty ("RegistrationType", 1) != XN_STATUS_OK) {
00145       throw Exception("Failed to set registration type");
00146     }
00147     */
00148     __cfg_copy_mode = CONVERT_YUV;
00149   }
00150 
00151   __image_md = new xn::ImageMetaData();
00152 
00153   __image_gen->GetMetaData(*__image_md);
00154 
00155   __image_width  = __image_md->XRes();
00156   __image_height = __image_md->YRes();
00157 
00158   /*
00159   const char *pixel_format = "unknown";
00160   switch (__image_gen->GetPixelFormat()) {
00161   case XN_PIXEL_FORMAT_RGB24:            pixel_format = "RGB24"; break;
00162   case XN_PIXEL_FORMAT_YUV422:           pixel_format = "YUV422"; break;
00163   case XN_PIXEL_FORMAT_GRAYSCALE_8_BIT:  pixel_format = "Gray8"; break;
00164   case XN_PIXEL_FORMAT_GRAYSCALE_16_BIT: pixel_format = "Gray16"; break;        
00165   }
00166 
00167   XnUInt64 input_format;
00168   if (__image_gen->GetIntProperty("InputFormat", input_format) != XN_STATUS_OK) {
00169     logger->log_warn(name(), "Failed to get input format");
00170   }
00171 
00172   logger->log_debug(name(), "Image  format: %s  width: %u  height: %u  input format: %lu",
00173                     pixel_format, __image_md->XRes(), __image_md->YRes(), input_format);
00174   */
00175 
00176   __image_buf_yuv =
00177     new SharedMemoryImageBuffer("openni-image-yuv", YUV422_PLANAR,
00178                                 __image_md->XRes(), __image_md->YRes());
00179 
00180   __image_buf_rgb =
00181     new SharedMemoryImageBuffer("openni-image-rgb", RGB,
00182                                 __image_md->XRes(), __image_md->YRes());
00183 
00184 
00185   __image_gen->StartGenerating();
00186 
00187   __capture_start = new Time(clock);
00188   __capture_start->stamp_systime();
00189   // Update once to get timestamp
00190   __image_gen->WaitAndUpdateData();
00191   // arbitrarily define the zero reference point,
00192   // we can't get any closer than this
00193   *__capture_start -= (long int)__image_gen->GetTimestamp();
00194   
00195   imagegen_autoptr.release();
00196 }
00197 
00198 
00199 void
00200 OpenNiImageThread::finalize()
00201 {
00202   // we do not stop generating, we don't know if there is no other plugin
00203   // using the node.
00204   delete __image_gen;
00205   delete __image_md;
00206   delete __image_buf_yuv;
00207   delete __image_buf_rgb;
00208 }
00209 
00210 
00211 void
00212 OpenNiImageThread::loop()
00213 {
00214   MutexLocker lock(openni.objmutex_ptr());
00215   bool is_image_new = __image_gen->IsDataNew();
00216   __image_gen->GetMetaData(*__image_md);
00217   const XnUInt8 * const      image_data = __image_md->Data();
00218   fawkes::Time ts = *__capture_start + (long int)__image_gen->GetTimestamp();
00219   lock.unlock();
00220 
00221   if (is_image_new && (__image_buf_yuv->num_attached() > 1)) {
00222     if (__cfg_copy_mode == DEBAYER_BILINEAR) {
00223       bayerGRBG_to_yuv422planar_bilinear(image_data, __image_buf_yuv->buffer(),
00224                                          __image_width, __image_height);
00225     } else if (__cfg_copy_mode == CONVERT_YUV) {
00226       yuv422packed_to_yuv422planar(image_data, __image_buf_yuv->buffer(),
00227                                    __image_width, __image_height);
00228     } else if (__cfg_copy_mode == DEBAYER_NEAREST_NEIGHBOR) {
00229       bayerGRBG_to_yuv422planar_nearest_neighbour(image_data,
00230                                                   __image_buf_yuv->buffer(),
00231                                                   __image_width, __image_height);
00232     }
00233     __image_buf_yuv->set_capture_time(&ts);
00234   }
00235 
00236   if (is_image_new && (__image_buf_rgb->num_attached() > 1)) {
00237     if (__cfg_copy_mode == DEBAYER_BILINEAR) {
00238       bayerGRBG_to_rgb_bilinear(image_data, __image_buf_rgb->buffer(),
00239                                 __image_width, __image_height);
00240     } else if (__cfg_copy_mode == CONVERT_YUV) {
00241       yuv422packed_to_rgb_plainc(image_data, __image_buf_rgb->buffer(),
00242                                  __image_width, __image_height);
00243     } else if (__cfg_copy_mode == DEBAYER_NEAREST_NEIGHBOR) {
00244       bayerGRBG_to_rgb_nearest_neighbour(image_data, __image_buf_rgb->buffer(),
00245                                          __image_width, __image_height);
00246     }
00247     __image_buf_rgb->set_capture_time(&ts);
00248   }
00249 }