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
37using namespace fawkes;
38using 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
60void
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 {
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
173void
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
185void
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}
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~OpenNiImageThread()
Destructor.
OpenNiImageThread()
Constructor.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
void unlock()
Unlock the mutex.
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:47
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:959
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:763
void unlock()
Unlock memory.
Definition: shm.cpp:1025
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:720
Shared memory image buffer.
Definition: shm_image.h:184
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:198
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
Fawkes library namespace.