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