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>
35 using namespace fawkes;
36 using namespace firevision;
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);