23 #include "image_thread.h"
24 #include "utils/setup.h"
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>
48 :
Thread(
"OpenNiImageThread",
Thread::OPMODE_WAITFORWAKEUP),
65 __cfg_copy_mode = CONVERT_YUV;
67 __image_gen =
new xn::ImageGenerator();
68 std::auto_ptr<xn::ImageGenerator> imagegen_autoptr(__image_gen);
72 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_IMAGE, __image_gen);
74 fawkes::openni::setup_map_generator(*__image_gen,
config);
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();
84 if ((pnd.Type == XN_NODE_TYPE_DEVICE) &&
85 (strcmp(pnd.strVendor,
"PrimeSense") == 0) &&
86 (strcmp(pnd.strName,
"SensorV2") == 0) )
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;
100 if ( (__usb_vendor == 0x045e) && (__usb_product == 0x02ae) ) {
105 if (__image_gen->SetIntProperty(
"InputFormat", 6) != XN_STATUS_OK) {
106 throw Exception(
"Failed to set uncompressed bayer input format");
108 if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT) != XN_STATUS_OK)
110 throw Exception(
"Failed to set pixel format");
119 __cfg_copy_mode = DEBAYER_BILINEAR;
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;
135 if (__image_gen->SetIntProperty(
"InputFormat", 5) != XN_STATUS_OK) {
136 throw Exception(
"Failed to set uncompressed bayer input format");
138 if (__image_gen->SetPixelFormat(XN_PIXEL_FORMAT_YUV422) != XN_STATUS_OK) {
139 throw Exception(
"Failed to set pixel format");
148 __cfg_copy_mode = CONVERT_YUV;
151 __image_md =
new xn::ImageMetaData();
153 __image_gen->GetMetaData(*__image_md);
155 __image_width = __image_md->XRes();
156 __image_height = __image_md->YRes();
178 __image_md->XRes(), __image_md->YRes());
182 __image_md->XRes(), __image_md->YRes());
185 __image_gen->StartGenerating();
190 __image_gen->WaitAndUpdateData();
193 *__capture_start -= (
long int)__image_gen->GetTimestamp();
195 imagegen_autoptr.release();
206 delete __image_buf_yuv;
207 delete __image_buf_rgb;
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();
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);
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);
LockPtr< xn::Context > openni
Central OpenNI context.
Time & stamp_systime()
Set this time to the current system time.
Fawkes library namespace.
Mutex * objmutex_ptr() const
Get object mutex.
A class for handling time.
virtual void loop()
Code to execute in the thread.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
Clock * clock
By means of this member access to the clock is given.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Shared memory image buffer.
unsigned char * buffer() const
Get image buffer.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
unsigned int num_attached() const
Get number of attached processes.
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.
void set_capture_time(fawkes::Time *time)
Set the capture time.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.