Fawkes API
Fawkes Development Version
|
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 }