23 #include "pointcloud_thread.h" 24 #include "image_thread.h" 25 #include "utils/setup.h" 27 #include <core/threading/mutex_locker.h> 28 #include <fvutils/ipc/shm_image.h> 29 #include <fvutils/color/colorspaces.h> 30 #include <fvutils/base/types.h> 31 #include <fvutils/color/rgb.h> 33 # include <pcl_utils/utils.h> 54 :
Thread(
"OpenNiPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
57 __img_thread = img_thread;
72 __image_rgb_buf = NULL;
74 __depth_gen =
new xn::DepthGenerator();
75 #if __cplusplus >= 201103L 76 std::unique_ptr<xn::DepthGenerator> depthgen_uniqueptr(__depth_gen);
77 std::unique_ptr<xn::ImageGenerator> imagegen_uniqueptr(__image_gen);
79 std::auto_ptr<xn::DepthGenerator> depthgen_uniqueptr(__depth_gen);
80 std::auto_ptr<xn::ImageGenerator> imagegen_uniqueptr(__image_gen);
83 __image_gen =
new xn::ImageGenerator();
87 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_DEPTH, __depth_gen);
88 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_IMAGE, __image_gen);
89 fawkes::openni::setup_map_generator(*__image_gen,
config);
90 fawkes::openni::setup_map_generator(*__depth_gen,
config);
92 __depth_md =
new xn::DepthMetaData();
93 __depth_gen->GetMetaData(*__depth_md);
95 __cfg_register_depth_image =
false;
97 __cfg_register_depth_image =
config->
get_bool(
"/plugins/openni/register_depth_image");
105 __depth_md->XRes(), __depth_md->YRes());
107 __pcl_xyz_buf->
set_frame_id(__cfg_register_depth_image ? __cfg_frame_image.c_str() : __cfg_frame_depth.c_str());
111 CARTESIAN_3D_FLOAT_RGB,
112 __depth_md->XRes(), __depth_md->YRes());
114 __pcl_xyzrgb_buf->
set_frame_id(__cfg_register_depth_image ? __cfg_frame_image.c_str() : __cfg_frame_depth.c_str());
119 if ((st = __depth_gen->GetIntProperty(
"ZPD", zpd)) != XN_STATUS_OK) {
120 throw Exception(
"Failed to get ZPD: %s", xnGetStatusString(st));
123 if ((st = __depth_gen->GetRealProperty(
"ZPPS", pixel_size)) != XN_STATUS_OK) {
124 throw Exception(
"Failed to get ZPPS: %s", xnGetStatusString(st));
127 if ((st = __depth_gen->GetIntProperty(
"NoSampleValue", __no_sample_value))
130 throw Exception(
"Failed to get NoSampleValue: %s", xnGetStatusString(st));
132 if ((st = __depth_gen->GetIntProperty(
"ShadowValue", __shadow_value))
135 throw Exception(
"Failed to get ShadowValue: %s", xnGetStatusString(st));
138 __width = __depth_md->XRes();
139 __height = __depth_md->YRes();
140 float scale = __width / (float)XN_SXGA_X_RES;
141 if (__cfg_register_depth_image) {
143 const float rgb_focal_length_SXGA = 1050;
144 __focal_length = rgb_focal_length_SXGA * scale;
146 __focal_length = ((float)zpd / pixel_size) * scale;
148 __foc_const = 0.001 / __focal_length;
149 __center_x = (__width / 2.) - .5f;
150 __center_y = (__height / 2.) - .5f;
152 __image_gen->StartGenerating();
153 __depth_gen->StartGenerating();
158 __depth_gen->WaitAndUpdateData();
161 *__capture_start -= (
long int)__depth_gen->GetTimestamp();
163 __image_gen->WaitAndUpdateData();
165 if (__cfg_register_depth_image) {
168 unsigned short usb_vendor = 0, usb_product = 0;
169 fawkes::openni::get_usb_info(*__depth_gen, usb_vendor, usb_product);
171 if ( (usb_vendor == 0x045e) && (usb_product == 0x02ae) ) {
172 if (__depth_gen->SetIntProperty(
"RegistrationType", 2) != XN_STATUS_OK) {
173 throw Exception(
"Failed to set registration type");
176 if (__depth_gen->SetIntProperty(
"RegistrationType", 1) != XN_STATUS_OK) {
177 throw Exception(
"Failed to set registration type");
182 fawkes::openni::setup_alternate_viewpoint(*__depth_gen, *__image_gen);
190 __cfg_generate_pcl =
true;
192 __cfg_generate_pcl =
config->
get_bool(
"/plugins/openni-pointcloud/generate-pcl");
195 if (__cfg_generate_pcl) {
197 __pcl_xyz->is_dense =
false;
198 __pcl_xyz->width = __width;
199 __pcl_xyz->height = __height;
200 __pcl_xyz->points.resize(__width * __height);
201 __pcl_xyz->header.frame_id = __cfg_register_depth_image ? __cfg_frame_image : __cfg_frame_depth;
204 __pcl_xyzrgb->is_dense =
false;
205 __pcl_xyzrgb->width = __width;
206 __pcl_xyzrgb->height = __height;
207 __pcl_xyzrgb->points.resize(__width * __height);
208 __pcl_xyzrgb->header.frame_id = __cfg_register_depth_image ? __cfg_frame_image : __cfg_frame_depth;
210 pcl_manager->add_pointcloud(
"openni-pointcloud-xyz", __pcl_xyz);
211 pcl_manager->add_pointcloud(
"openni-pointcloud-xyzrgb", __pcl_xyzrgb);
215 depthgen_uniqueptr.release();
216 imagegen_uniqueptr.release();
224 pcl_manager->remove_pointcloud(
"openni-pointcloud-xyz");
225 pcl_manager->remove_pointcloud(
"openni-pointcloud-xyzrgb");
232 delete __pcl_xyz_buf;
233 delete __pcl_xyzrgb_buf;
234 delete __capture_start;
239 OpenNiPointCloudThread::fill_xyz_no_pcl(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
246 unsigned int idx = 0;
247 for (
unsigned int h = 0; h < __height; ++h) {
248 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
250 if (depth_data[idx] == 0 ||
251 depth_data[idx] == __no_sample_value ||
252 depth_data[idx] == __shadow_value)
255 pclbuf->
x = pclbuf->
y = pclbuf->
z = 0.f;
258 pclbuf->
x = depth_data[idx] * 0.001f;
259 pclbuf->
y = -(w - __center_x) * depth_data[idx] * __foc_const;
260 pclbuf->
z = -(h - __center_y) * depth_data[idx] * __foc_const;
270 OpenNiPointCloudThread::fill_xyzrgb_no_pcl(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
277 unsigned int idx = 0;
278 for (
unsigned int h = 0; h < __height; ++h) {
279 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
281 if (depth_data[idx] == 0 ||
282 depth_data[idx] == __no_sample_value ||
283 depth_data[idx] == __shadow_value)
286 pclbuf_rgb->
x = pclbuf_rgb->
y = pclbuf_rgb->
z = 0.f;
289 pclbuf_rgb->
x = depth_data[idx] * 0.001f;
290 pclbuf_rgb->
y = -(w - __center_x) * depth_data[idx] * __foc_const;
291 pclbuf_rgb->
z = -(h - __center_y) * depth_data[idx] * __foc_const;
298 __pcl_xyzrgb_buf->
unlock();
303 OpenNiPointCloudThread::fill_xyz_xyzrgb_no_pcl(
fawkes::Time &ts,
304 const XnDepthPixel *
const depth_data)
315 unsigned int idx = 0;
316 for (
unsigned int h = 0; h < __height; ++h) {
317 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
319 if (depth_data[idx] == 0 ||
320 depth_data[idx] == __no_sample_value ||
321 depth_data[idx] == __shadow_value)
324 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
325 pclbuf_xyz->
x = pclbuf_xyz->
y = pclbuf_xyz->
z = 0.f;
328 pclbuf_rgb->x = pclbuf_xyz->
x = depth_data[idx] * 0.001f;
329 pclbuf_rgb->y = pclbuf_xyz->
y = -(w - __center_x) * depth_data[idx] * __foc_const;
330 pclbuf_rgb->z = pclbuf_xyz->
z = -(h - __center_y) * depth_data[idx] * __foc_const;
337 __pcl_xyzrgb_buf->
unlock();
343 OpenNiPointCloudThread::fill_rgb_no_pcl()
345 if (! __image_rgb_buf) {
359 for (
unsigned int i = 0; i < __width * __height; ++i) {
360 pclbuf_rgb->
r = imagebuf[i].
R;
361 pclbuf_rgb->
g = imagebuf[i].
G;
362 pclbuf_rgb->
b = imagebuf[i].
B;
370 OpenNiPointCloudThread::fill_xyz(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
374 pcl_utils::set_time(__pcl_xyz, ts);
381 unsigned int idx = 0;
382 for (
unsigned int h = 0; h < __height; ++h) {
383 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
385 if (depth_data[idx] == 0 ||
386 depth_data[idx] == __no_sample_value ||
387 depth_data[idx] == __shadow_value)
390 pclbuf->
x = pclbuf->
y = pclbuf->
z = 0.f;
391 pcl.points[idx].x = pcl.points[idx].y = pcl.points[idx].z = 0.f;
394 pclbuf->
x = pcl.points[idx].x = depth_data[idx] * 0.001f;
395 pclbuf->
y = pcl.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
396 pclbuf->
z = pcl.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
406 OpenNiPointCloudThread::fill_xyzrgb(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
409 pcl_rgb.header.seq += 1;
410 pcl_utils::set_time(__pcl_xyzrgb, ts);
417 unsigned int idx = 0;
418 for (
unsigned int h = 0; h < __height; ++h) {
419 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
421 if (depth_data[idx] == 0 ||
422 depth_data[idx] == __no_sample_value ||
423 depth_data[idx] == __shadow_value)
426 pclbuf_rgb->
x = pclbuf_rgb->
y = pclbuf_rgb->
z = 0.f;
427 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
430 pclbuf_rgb->
x = pcl_rgb.points[idx].x = depth_data[idx] * 0.001f;
431 pclbuf_rgb->
y = pcl_rgb.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
432 pclbuf_rgb->
z = pcl_rgb.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
439 __pcl_xyzrgb_buf->
unlock();
444 OpenNiPointCloudThread::fill_xyz_xyzrgb(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
448 pcl_rgb.header.seq += 1;
449 pcl_utils::set_time(__pcl_xyzrgb, ts);
452 pcl_xyz.header.seq += 1;
453 pcl_utils::set_time(__pcl_xyz, ts);
464 unsigned int idx = 0;
465 for (
unsigned int h = 0; h < __height; ++h) {
466 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
468 if (depth_data[idx] == 0 ||
469 depth_data[idx] == __no_sample_value ||
470 depth_data[idx] == __shadow_value)
473 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
474 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
476 pclbuf_xyz->
x = pclbuf_xyz->
y = pclbuf_xyz->
z = 0.f;
477 pcl_xyz.points[idx].x = pcl_xyz.points[idx].y = pcl_xyz.points[idx].z = 0.f;
480 pclbuf_rgb->x = pcl_rgb.points[idx].x = pclbuf_xyz->
x = pcl_xyz.points[idx].x =
481 depth_data[idx] * 0.001f;
482 pclbuf_rgb->y = pcl_rgb.points[idx].y = pclbuf_xyz->
y = pcl_xyz.points[idx].y =
483 -(w - __center_x) * depth_data[idx] * __foc_const;
484 pclbuf_rgb->z = pcl_rgb.points[idx].z = pclbuf_xyz->
z = pcl_xyz.points[idx].z =
485 -(h - __center_y) * depth_data[idx] * __foc_const;
492 __pcl_xyzrgb_buf->
unlock();
500 if (! __image_rgb_buf) {
514 for (
unsigned int i = 0; i < __width * __height; ++i) {
515 pclbuf_rgb->
r = pcl_rgb.points[i].r = imagebuf[i].
R;
516 pclbuf_rgb->
g = pcl_rgb.points[i].g = imagebuf[i].
G;
517 pclbuf_rgb->
b = pcl_rgb.points[i].b = imagebuf[i].
B;
528 bool is_data_new = __depth_gen->IsDataNew();
529 __depth_gen->GetMetaData(*__depth_md);
530 const XnDepthPixel *
const data = __depth_md->Data();
535 bool xyz_requested = (__pcl_xyz_buf->
num_attached() > 1)
538 || (__cfg_generate_pcl && ((__pcl_xyz.use_count() > 2)))
541 bool xyzrgb_requested = (__pcl_xyzrgb_buf->
num_attached() > 1)
544 || (__cfg_generate_pcl && ((__pcl_xyzrgb.use_count() > 2)))
548 if (is_data_new && (xyz_requested || xyzrgb_requested)) {
550 fawkes::Time ts = *__capture_start + (
long int)__depth_gen->GetTimestamp();
553 if (__cfg_generate_pcl) {
555 if (xyz_requested && xyzrgb_requested) {
556 fill_xyz_xyzrgb(ts, data);
557 }
else if (xyz_requested) {
559 }
else if (xyzrgb_requested) {
560 fill_xyzrgb(ts, data);
565 if (xyz_requested && xyzrgb_requested) {
566 fill_xyz_xyzrgb_no_pcl(ts, data);
567 }
else if (xyz_requested) {
568 fill_xyz_no_pcl(ts, data);
569 }
else if (xyzrgb_requested) {
570 fill_xyzrgb_no_pcl(ts, data);
577 if (! xyzrgb_requested && __image_rgb_buf) {
578 delete __image_rgb_buf;
579 __image_rgb_buf = NULL;
LockPtr< xn::Context > openni
Central OpenNI context.
Structure defining an RGB pixel (in R-G-B byte ordering).
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Time & stamp_systime()
Set this time to the current system time.
void lock_for_write()
Lock shared memory segment for writing.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
uint8_t b
B color component value.
Structure defining a point in a CARTESIAN_3D_FLOAT_RGB buffer.
A class for handling time.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void init()
Initialize the thread.
void wait_loop_done()
Wait for the current loop iteration to finish.
unsigned char * buffer() const
Get image buffer.
uint8_t r
R color component value.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Mutex * objmutex_ptr() const
Get object mutex.
void set_frame_id(const char *frame_id)
Set frame ID.
virtual void finalize()
Finalize the thread.
uint8_t g
G color component value.
virtual ~OpenNiPointCloudThread()
Destructor.
Shared memory image buffer.
unsigned int num_attached() const
Get number of attached processes.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void loop()
Code to execute in the thread.
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
OpenNI Image Provider Thread.
OpenNiPointCloudThread(OpenNiImageThread *img_thread)
Constructor.
Configuration * config
This is the Configuration member used to access the configuration.
void set_capture_time(fawkes::Time *time)
Set the capture time.
void unlock()
Unlock memory.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.