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 #include <fvutils/color/rgbyuv.h>
33 
34 #include <memory>
35 
36 using namespace fawkes;
37 using namespace firevision;
38 
39 /** @class OpenNiImageThread "image_thread.h"
40  * OpenNI Image Provider Thread.
41  * This thread provides YUV and RGB images from the camera via
42  * SharedMemoryImageBuffer to other FireVision plugins.
43  *
44  * @author Tim Niemueller
45  */
46 
47 /** Constructor. */
49  : Thread("OpenNiImageThread", Thread::OPMODE_WAITFORWAKEUP),
50  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
51 {
52 }
53 
54 
55 /** Destructor. */
57 {
58 }
59 
60 
61 void
63 {
65 
66  __cfg_copy_mode = CONVERT_YUV;
67 
68  __image_gen = new xn::ImageGenerator();
69 #if __cplusplus >= 201103L
70  std::unique_ptr<xn::ImageGenerator> imagegen_uniqueptr(__image_gen);
71 #else
72  std::auto_ptr<xn::ImageGenerator> imagegen_uniqueptr(__image_gen);
73 #endif
74 
75  XnStatus st;
76 
77  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_IMAGE, __image_gen);
78 
79  fawkes::openni::setup_map_generator(*__image_gen, config);
80 
81  fawkes::openni::get_usb_info(*__image_gen, __usb_vendor, __usb_product);
82 
83  if ( (__usb_vendor == 0x045e) && (__usb_product == 0x02ae) ) {
84  // from OpenNI-PrimeSense/XnStreamParams.h:
85  // XN_IO_IMAGE_FORMAT_UNCOMPRESSED_BAYER = 6
86  // InputFormat should be 6 = uncompressed Bayer for Kinect
87  logger->log_debug(name(), "Kinect camera detected, initializing");
88  if (__image_gen->SetIntProperty("InputFormat", 6) != XN_STATUS_OK) {
89  throw Exception("Failed to set uncompressed bayer input format");
90  }
91  if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT) != XN_STATUS_OK)
92  {
93  throw Exception("Failed to set pixel format");
94  }
95  /*
96  // RegistrationType should be 2 (software) for Kinect, 1 (hardware) for PS
97  // (from ROS openni_camera)
98  if (__depth_gen->SetIntProperty ("RegistrationType", 2) != XN_STATUS_OK) {
99  throw Exception("Failed to set registration type");
100  }
101  */
102  __cfg_copy_mode = DEBAYER_BILINEAR;
103  try {
104  std::string debayering = config->get_string("/plugins/openni-image/debayering");
105  if (debayering == "bilinear") {
106  __cfg_copy_mode = DEBAYER_BILINEAR;
107  } else if (debayering == "nearest_neighbor") {
108  __cfg_copy_mode = DEBAYER_NEAREST_NEIGHBOR;
109  } else {
110  logger->log_warn(name(), "Unknown de-bayering mode '%s', using bilinear instead.",
111  debayering.c_str());
112  }
113  } catch (Exception &e) {
114  logger->log_warn(name(), "No de-bayering mode set, using bilinear.");
115  }
116  } else {
117  logger->log_debug(name(), "PrimeSense camera detected, initializing");
118  if (__image_gen->SetIntProperty("InputFormat", 5) != XN_STATUS_OK) {
119  throw Exception("Failed to set uncompressed bayer input format");
120  }
121  if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_YUV422) != XN_STATUS_OK) {
122  throw Exception("Failed to set pixel format");
123  }
124  __cfg_copy_mode = CONVERT_YUV;
125  }
126 
127  __image_md = new xn::ImageMetaData();
128 
129  __image_gen->GetMetaData(*__image_md);
130 
131  __image_width = __image_md->XRes();
132  __image_height = __image_md->YRes();
133 
134  /*
135  const char *pixel_format = "unknown";
136  switch (__image_gen->GetPixelFormat()) {
137  case XN_PIXEL_FORMAT_RGB24: pixel_format = "RGB24"; __cfg_copy_mode = CONVERT_RGB; break;
138  case XN_PIXEL_FORMAT_YUV422: pixel_format = "YUV422"; break;
139  case XN_PIXEL_FORMAT_GRAYSCALE_8_BIT: pixel_format = "Gray8"; break;
140  case XN_PIXEL_FORMAT_GRAYSCALE_16_BIT: pixel_format = "Gray16"; break;
141  case XN_PIXEL_FORMAT_MJPEG: pixel_format = "MJPEG"; break;
142  }
143 
144  XnUInt64 input_format;
145  if (__image_gen->GetIntProperty("InputFormat", input_format) != XN_STATUS_OK) {
146  logger->log_warn(name(), "Failed to get input format");
147  }
148 
149  logger->log_debug(name(), "Image format: %s width: %u height: %u input format: %lu",
150  pixel_format, __image_md->XRes(), __image_md->YRes(), input_format);
151  */
152 
153  __image_buf_yuv =
154  new SharedMemoryImageBuffer("openni-image-yuv", YUV422_PLANAR,
155  __image_md->XRes(), __image_md->YRes());
156 
157  __image_buf_rgb =
158  new SharedMemoryImageBuffer("openni-image-rgb", RGB,
159  __image_md->XRes(), __image_md->YRes());
160 
161 
162  __image_gen->StartGenerating();
163 
164  __capture_start = new Time(clock);
165  __capture_start->stamp_systime();
166  // Update once to get timestamp
167  __image_gen->WaitAndUpdateData();
168  // arbitrarily define the zero reference point,
169  // we can't get any closer than this
170  *__capture_start -= (long int)__image_gen->GetTimestamp();
171 
172  imagegen_uniqueptr.release();
173 }
174 
175 
176 void
178 {
179  // we do not stop generating, we don't know if there is no other plugin
180  // using the node.
181  delete __image_gen;
182  delete __image_md;
183  delete __image_buf_yuv;
184  delete __image_buf_rgb;
185 }
186 
187 
188 void
190 {
192  bool is_image_new = __image_gen->IsDataNew();
193  __image_gen->GetMetaData(*__image_md);
194  const XnUInt8 * const image_data = __image_md->Data();
195  fawkes::Time ts = *__capture_start + (long int)__image_gen->GetTimestamp();
196  lock.unlock();
197 
198  if (is_image_new && (__image_buf_yuv->num_attached() > 1)) {
199  __image_buf_yuv->lock_for_write();
200  if (__cfg_copy_mode == DEBAYER_BILINEAR) {
201  bayerGRBG_to_yuv422planar_bilinear(image_data, __image_buf_yuv->buffer(),
202  __image_width, __image_height);
203  } else if (__cfg_copy_mode == CONVERT_YUV) {
204  yuv422packed_to_yuv422planar(image_data, __image_buf_yuv->buffer(),
205  __image_width, __image_height);
206  } else if (__cfg_copy_mode == CONVERT_RGB) {
207  rgb_to_yuv422planar_plainc(image_data, __image_buf_yuv->buffer(),
208  __image_width, __image_height);
209  } else if (__cfg_copy_mode == DEBAYER_NEAREST_NEIGHBOR) {
210  bayerGRBG_to_yuv422planar_nearest_neighbour(image_data,
211  __image_buf_yuv->buffer(),
212  __image_width, __image_height);
213  }
214  __image_buf_yuv->set_capture_time(&ts);
215  __image_buf_yuv->unlock();
216  }
217 
218  if (is_image_new && (__image_buf_rgb->num_attached() > 1)) {
219  __image_buf_rgb->lock_for_write();
220  if (__cfg_copy_mode == DEBAYER_BILINEAR) {
221  bayerGRBG_to_rgb_bilinear(image_data, __image_buf_rgb->buffer(),
222  __image_width, __image_height);
223  } else if (__cfg_copy_mode == CONVERT_YUV) {
224  yuv422packed_to_rgb_plainc(image_data, __image_buf_rgb->buffer(),
225  __image_width, __image_height);
226  } else if (__cfg_copy_mode == CONVERT_RGB) {
227  memcpy(__image_buf_rgb->buffer(), image_data,
228  colorspace_buffer_size(RGB, __image_width, __image_height));
229  } else if (__cfg_copy_mode == DEBAYER_NEAREST_NEIGHBOR) {
230  bayerGRBG_to_rgb_nearest_neighbour(image_data, __image_buf_rgb->buffer(),
231  __image_width, __image_height);
232  }
233  __image_buf_rgb->set_capture_time(&ts);
234  __image_buf_rgb->unlock();
235  }
236 }
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
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:918
Fawkes library namespace.
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
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:235
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
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:262
Shared memory image buffer.
Definition: shm_image.h:181
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:714
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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:205
void unlock()
Unlock memory.
Definition: shm.cpp:985
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.