Fawkes API  Fawkes Development Version
image_thread.cpp
1 
2 /***************************************************************************
3  * image_thread.cpp - OpenNI image provider thread
4  *
5  * Created: Thu Mar 17 14:06:39 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "image_thread.h"
24 #include "utils/setup.h"
25 
26 #include <core/threading/mutex_locker.h>
27 #include <fvutils/ipc/shm_image.h>
28 #include <fvutils/color/colorspaces.h>
29 #include <fvutils/color/bayer.h>
30 #include <fvutils/color/yuv.h>
31 #include <fvutils/color/yuvrgb.h>
32 
33 #include <memory>
34 
35 using namespace fawkes;
36 using namespace firevision;
37 
38 /** @class OpenNiImageThread "image_thread.h"
39  * OpenNI Image Provider Thread.
40  * This thread provides YUV and RGB images from the camera via
41  * SharedMemoryImageBuffer to other FireVision plugins.
42  *
43  * @author Tim Niemueller
44  */
45 
46 /** Constructor. */
48  : Thread("OpenNiImageThread", Thread::OPMODE_WAITFORWAKEUP),
49  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
50 {
51 }
52 
53 
54 /** Destructor. */
56 {
57 }
58 
59 
60 void
62 {
64 
65  __cfg_copy_mode = CONVERT_YUV;
66 
67  __image_gen = new xn::ImageGenerator();
68  std::auto_ptr<xn::ImageGenerator> imagegen_autoptr(__image_gen);
69 
70  XnStatus st;
71 
72  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_IMAGE, __image_gen);
73 
74  fawkes::openni::setup_map_generator(*__image_gen, config);
75 
76  __usb_vendor = 0;
77  __usb_product = 0;
78 
79  xn::NodeInfo image_info = __image_gen->GetInfo();
80  xn::NodeInfoList &depnodes = image_info.GetNeededNodes();
81  for (xn::NodeInfoList::Iterator n = depnodes.Begin(); n != depnodes.End(); ++n) {
82  const XnProductionNodeDescription &pnd = (*n).GetDescription();
83 
84  if ((pnd.Type == XN_NODE_TYPE_DEVICE) &&
85  (strcmp(pnd.strVendor, "PrimeSense") == 0) &&
86  (strcmp(pnd.strName, "SensorV2") == 0) )
87  {
88  // it's the primesense device node and we can check for USB vendor/product
89  unsigned short int vendor = 0, product = 0;
90  unsigned char bus = 0, addr = 0;
91  if (sscanf((*n).GetCreationInfo(), "%04hx/%04hx@%hhu/%hhu", &vendor, &product, &bus, &addr) == 4) {
92  logger->log_debug(name(), "Detected USB device (vendor: %04hx product: %04hx bus: %hhu addr: %hhu)",
93  vendor, product, bus, addr);
94  __usb_vendor = vendor;
95  __usb_product = product;
96  }
97  }
98  }
99 
100  if ( (__usb_vendor == 0x045e) && (__usb_product == 0x02ae) ) {
101  // from OpenNI-PrimeSense/XnStreamParams.h:
102  // XN_IO_IMAGE_FORMAT_UNCOMPRESSED_BAYER = 6
103  // InputFormat should be 6 = uncompressed Bayer for Kinect
104  logger->log_debug(name(), "Kinect camera detected, initializing");
105  if (__image_gen->SetIntProperty("InputFormat", 6) != XN_STATUS_OK) {
106  throw Exception("Failed to set uncompressed bayer input format");
107  }
108  if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT) != XN_STATUS_OK)
109  {
110  throw Exception("Failed to set pixel format");
111  }
112  /*
113  // RegistrationType should be 2 (software) for Kinect, 1 (hardware) for PS
114  // (from ROS openni_camera)
115  if (__depth_gen->SetIntProperty ("RegistrationType", 2) != XN_STATUS_OK) {
116  throw Exception("Failed to set registration type");
117  }
118  */
119  __cfg_copy_mode = DEBAYER_BILINEAR;
120  try {
121  std::string debayering = config->get_string("/plugins/openni-image/debayering");
122  if (debayering == "bilinear") {
123  __cfg_copy_mode = DEBAYER_BILINEAR;
124  } else if (debayering == "nearest_neighbor") {
125  __cfg_copy_mode = DEBAYER_NEAREST_NEIGHBOR;
126  } else {
127  logger->log_warn(name(), "Unknown de-bayering mode '%s', using bilinear instead.",
128  debayering.c_str());
129  }
130  } catch (Exception &e) {
131  logger->log_warn(name(), "No de-bayering mode set, using bilinear.");
132  }
133  } else {
134  logger->log_debug(name(), "PrimeSense camera detected, initializing");
135  if (__image_gen->SetIntProperty("InputFormat", 5) != XN_STATUS_OK) {
136  throw Exception("Failed to set uncompressed bayer input format");
137  }
138  if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_YUV422) != XN_STATUS_OK) {
139  throw Exception("Failed to set pixel format");
140  }
141  /*
142  // RegistrationType should be 2 (software) for Kinect, 1 (hardware) for PS
143  // (from ROS openni_camera)
144  if (__depth_gen->SetIntProperty ("RegistrationType", 1) != XN_STATUS_OK) {
145  throw Exception("Failed to set registration type");
146  }
147  */
148  __cfg_copy_mode = CONVERT_YUV;
149  }
150 
151  __image_md = new xn::ImageMetaData();
152 
153  __image_gen->GetMetaData(*__image_md);
154 
155  __image_width = __image_md->XRes();
156  __image_height = __image_md->YRes();
157 
158  /*
159  const char *pixel_format = "unknown";
160  switch (__image_gen->GetPixelFormat()) {
161  case XN_PIXEL_FORMAT_RGB24: pixel_format = "RGB24"; break;
162  case XN_PIXEL_FORMAT_YUV422: pixel_format = "YUV422"; break;
163  case XN_PIXEL_FORMAT_GRAYSCALE_8_BIT: pixel_format = "Gray8"; break;
164  case XN_PIXEL_FORMAT_GRAYSCALE_16_BIT: pixel_format = "Gray16"; break;
165  }
166 
167  XnUInt64 input_format;
168  if (__image_gen->GetIntProperty("InputFormat", input_format) != XN_STATUS_OK) {
169  logger->log_warn(name(), "Failed to get input format");
170  }
171 
172  logger->log_debug(name(), "Image format: %s width: %u height: %u input format: %lu",
173  pixel_format, __image_md->XRes(), __image_md->YRes(), input_format);
174  */
175 
176  __image_buf_yuv =
177  new SharedMemoryImageBuffer("openni-image-yuv", YUV422_PLANAR,
178  __image_md->XRes(), __image_md->YRes());
179 
180  __image_buf_rgb =
181  new SharedMemoryImageBuffer("openni-image-rgb", RGB,
182  __image_md->XRes(), __image_md->YRes());
183 
184 
185  __image_gen->StartGenerating();
186 
187  __capture_start = new Time(clock);
188  __capture_start->stamp_systime();
189  // Update once to get timestamp
190  __image_gen->WaitAndUpdateData();
191  // arbitrarily define the zero reference point,
192  // we can't get any closer than this
193  *__capture_start -= (long int)__image_gen->GetTimestamp();
194 
195  imagegen_autoptr.release();
196 }
197 
198 
199 void
201 {
202  // we do not stop generating, we don't know if there is no other plugin
203  // using the node.
204  delete __image_gen;
205  delete __image_md;
206  delete __image_buf_yuv;
207  delete __image_buf_rgb;
208 }
209 
210 
211 void
213 {
215  bool is_image_new = __image_gen->IsDataNew();
216  __image_gen->GetMetaData(*__image_md);
217  const XnUInt8 * const image_data = __image_md->Data();
218  fawkes::Time ts = *__capture_start + (long int)__image_gen->GetTimestamp();
219  lock.unlock();
220 
221  if (is_image_new && (__image_buf_yuv->num_attached() > 1)) {
222  if (__cfg_copy_mode == DEBAYER_BILINEAR) {
223  bayerGRBG_to_yuv422planar_bilinear(image_data, __image_buf_yuv->buffer(),
224  __image_width, __image_height);
225  } else if (__cfg_copy_mode == CONVERT_YUV) {
226  yuv422packed_to_yuv422planar(image_data, __image_buf_yuv->buffer(),
227  __image_width, __image_height);
228  } else if (__cfg_copy_mode == DEBAYER_NEAREST_NEIGHBOR) {
229  bayerGRBG_to_yuv422planar_nearest_neighbour(image_data,
230  __image_buf_yuv->buffer(),
231  __image_width, __image_height);
232  }
233  __image_buf_yuv->set_capture_time(&ts);
234  }
235 
236  if (is_image_new && (__image_buf_rgb->num_attached() > 1)) {
237  if (__cfg_copy_mode == DEBAYER_BILINEAR) {
238  bayerGRBG_to_rgb_bilinear(image_data, __image_buf_rgb->buffer(),
239  __image_width, __image_height);
240  } else if (__cfg_copy_mode == CONVERT_YUV) {
241  yuv422packed_to_rgb_plainc(image_data, __image_buf_rgb->buffer(),
242  __image_width, __image_height);
243  } else if (__cfg_copy_mode == DEBAYER_NEAREST_NEIGHBOR) {
244  bayerGRBG_to_rgb_nearest_neighbour(image_data, __image_buf_rgb->buffer(),
245  __image_width, __image_height);
246  }
247  __image_buf_rgb->set_capture_time(&ts);
248  }
249 }
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:48
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:800
Fawkes library namespace.
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:240
Mutex locking helper.
Definition: mutex_locker.h:33
A class for handling time.
Definition: time.h:91
virtual void loop()
Code to execute in the thread.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Shared memory image buffer.
Definition: shm_image.h:135
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:234
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Definition: thread.h:95
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:714
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
OpenNiImageThread()
Constructor.
virtual ~OpenNiImageThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:204
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.