23 #include "pointcloud_thread.h"
25 #include "image_thread.h"
26 #include "utils/setup.h"
28 #include <core/threading/mutex_locker.h>
29 #include <fvutils/base/types.h>
30 #include <fvutils/color/colorspaces.h>
31 #include <fvutils/color/rgb.h>
32 #include <fvutils/ipc/shm_image.h>
34 # include <pcl_utils/utils.h>
40 using namespace firevision;
55 :
Thread(
"OpenNiPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
58 img_thread_ = img_thread;
71 image_rgb_buf_ = NULL;
73 depth_gen_ =
new xn::DepthGenerator();
74 #if __cplusplus >= 201103L
75 std::unique_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
76 std::unique_ptr<xn::ImageGenerator> imagegen_uniqueptr(image_gen_);
78 std::auto_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
79 std::auto_ptr<xn::ImageGenerator> imagegen_uniqueptr(image_gen_);
82 image_gen_ =
new xn::ImageGenerator();
86 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_DEPTH, depth_gen_);
87 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_IMAGE, image_gen_);
88 fawkes::openni::setup_map_generator(*image_gen_,
config);
89 fawkes::openni::setup_map_generator(*depth_gen_,
config);
91 depth_md_ =
new xn::DepthMetaData();
92 depth_gen_->GetMetaData(*depth_md_);
94 cfg_register_depth_image_ =
false;
96 cfg_register_depth_image_ =
config->
get_bool(
"/plugins/openni/register_depth_image");
108 pcl_xyz_buf_->
set_frame_id(cfg_register_depth_image_ ? cfg_frame_image_.c_str()
109 : cfg_frame_depth_.c_str());
112 CARTESIAN_3D_FLOAT_RGB,
116 pcl_xyzrgb_buf_->
set_frame_id(cfg_register_depth_image_ ? cfg_frame_image_.c_str()
117 : cfg_frame_depth_.c_str());
122 if ((st = depth_gen_->GetIntProperty(
"ZPD", zpd)) != XN_STATUS_OK) {
123 throw Exception(
"Failed to get ZPD: %s", xnGetStatusString(st));
126 if ((st = depth_gen_->GetRealProperty(
"ZPPS", pixel_size)) != XN_STATUS_OK) {
127 throw Exception(
"Failed to get ZPPS: %s", xnGetStatusString(st));
130 if ((st = depth_gen_->GetIntProperty(
"NoSampleValue", no_sample_value_)) != XN_STATUS_OK) {
131 throw Exception(
"Failed to get NoSampleValue: %s", xnGetStatusString(st));
133 if ((st = depth_gen_->GetIntProperty(
"ShadowValue", shadow_value_)) != XN_STATUS_OK) {
134 throw Exception(
"Failed to get ShadowValue: %s", xnGetStatusString(st));
137 width_ = depth_md_->XRes();
138 height_ = depth_md_->YRes();
139 float scale = width_ / (float)XN_SXGA_X_RES;
140 if (cfg_register_depth_image_) {
142 const float rgb_focal_length_SXGA = 1050;
143 focal_length_ = rgb_focal_length_SXGA * scale;
145 focal_length_ = ((float)zpd / pixel_size) * scale;
147 foc_const_ = 0.001 / focal_length_;
148 center_x_ = (width_ / 2.) - .5f;
149 center_y_ = (height_ / 2.) - .5f;
151 image_gen_->StartGenerating();
152 depth_gen_->StartGenerating();
157 depth_gen_->WaitAndUpdateData();
160 *capture_start_ -= (
long int)depth_gen_->GetTimestamp();
162 image_gen_->WaitAndUpdateData();
164 if (cfg_register_depth_image_) {
167 unsigned short usb_vendor = 0, usb_product = 0;
168 fawkes::openni::get_usb_info(*depth_gen_, usb_vendor, usb_product);
170 if ((usb_vendor == 0x045e) && (usb_product == 0x02ae)) {
171 if (depth_gen_->SetIntProperty(
"RegistrationType", 2) != XN_STATUS_OK) {
172 throw Exception(
"Failed to set registration type");
175 if (depth_gen_->SetIntProperty(
"RegistrationType", 1) != XN_STATUS_OK) {
176 throw Exception(
"Failed to set registration type");
181 fawkes::openni::setup_alternate_viewpoint(*depth_gen_, *image_gen_);
189 cfg_generate_pcl_ =
true;
191 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((
size_t)width_ * (
size_t)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((
size_t)width_ * (
size_t)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();
223 pcl_manager->remove_pointcloud(
"openni-pointcloud-xyz");
224 pcl_manager->remove_pointcloud(
"openni-pointcloud-xyzrgb");
232 delete pcl_xyzrgb_buf_;
233 delete capture_start_;
237 OpenNiPointCloudThread::fill_xyz_no_pcl(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
244 unsigned int idx = 0;
245 for (
unsigned int h = 0; h < height_; ++h) {
246 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf) {
248 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
249 || depth_data[idx] == shadow_value_) {
251 pclbuf->
x = pclbuf->
y = pclbuf->
z = 0.f;
254 pclbuf->
x = depth_data[idx] * 0.001f;
255 pclbuf->
y = -(w - center_x_) * depth_data[idx] * foc_const_;
256 pclbuf->
z = -(h - center_y_) * depth_data[idx] * foc_const_;
265 OpenNiPointCloudThread::fill_xyzrgb_no_pcl(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
272 unsigned int idx = 0;
273 for (
unsigned int h = 0; h < height_; ++h) {
274 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb) {
276 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
277 || depth_data[idx] == shadow_value_) {
279 pclbuf_rgb->
x = pclbuf_rgb->
y = pclbuf_rgb->
z = 0.f;
282 pclbuf_rgb->
x = depth_data[idx] * 0.001f;
283 pclbuf_rgb->
y = -(w - center_x_) * depth_data[idx] * foc_const_;
284 pclbuf_rgb->
z = -(h - center_y_) * depth_data[idx] * foc_const_;
291 pcl_xyzrgb_buf_->
unlock();
295 OpenNiPointCloudThread::fill_xyz_xyzrgb_no_pcl(
fawkes::Time & ts,
296 const XnDepthPixel *
const depth_data)
307 unsigned int idx = 0;
308 for (
unsigned int h = 0; h < height_; ++h) {
309 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
311 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
312 || depth_data[idx] == shadow_value_) {
314 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
315 pclbuf_xyz->
x = pclbuf_xyz->
y = pclbuf_xyz->
z = 0.f;
318 pclbuf_rgb->x = pclbuf_xyz->
x = depth_data[idx] * 0.001f;
319 pclbuf_rgb->y = pclbuf_xyz->
y = -(w - center_x_) * depth_data[idx] * foc_const_;
320 pclbuf_rgb->z = pclbuf_xyz->
z = -(h - center_y_) * depth_data[idx] * foc_const_;
327 pcl_xyzrgb_buf_->
unlock();
332 OpenNiPointCloudThread::fill_rgb_no_pcl()
334 if (!image_rgb_buf_) {
348 for (
unsigned int i = 0; i < width_ * height_; ++i) {
349 pclbuf_rgb->
r = imagebuf[i].
R;
350 pclbuf_rgb->
g = imagebuf[i].
G;
351 pclbuf_rgb->
b = imagebuf[i].
B;
357 OpenNiPointCloudThread::fill_xyz(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
361 pcl_utils::set_time(pcl_xyz_, ts);
368 unsigned int idx = 0;
369 for (
unsigned int h = 0; h < height_; ++h) {
370 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf) {
372 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
373 || depth_data[idx] == shadow_value_) {
375 pclbuf->
x = pclbuf->
y = pclbuf->
z = 0.f;
376 pcl.points[idx].x = pcl.points[idx].y = pcl.points[idx].z = 0.f;
379 pclbuf->
x = pcl.points[idx].x = depth_data[idx] * 0.001f;
380 pclbuf->
y = pcl.points[idx].y = -(w - center_x_) * depth_data[idx] * foc_const_;
381 pclbuf->
z = pcl.points[idx].z = -(h - center_y_) * depth_data[idx] * foc_const_;
390 OpenNiPointCloudThread::fill_xyzrgb(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
393 pcl_rgb.header.seq += 1;
394 pcl_utils::set_time(pcl_xyzrgb_, ts);
401 unsigned int idx = 0;
402 for (
unsigned int h = 0; h < height_; ++h) {
403 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb) {
405 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
406 || depth_data[idx] == shadow_value_) {
408 pclbuf_rgb->
x = pclbuf_rgb->
y = pclbuf_rgb->
z = 0.f;
409 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
412 pclbuf_rgb->
x = pcl_rgb.points[idx].x = depth_data[idx] * 0.001f;
413 pclbuf_rgb->
y = pcl_rgb.points[idx].y = -(w - center_x_) * depth_data[idx] * foc_const_;
414 pclbuf_rgb->
z = pcl_rgb.points[idx].z = -(h - center_y_) * depth_data[idx] * foc_const_;
421 pcl_xyzrgb_buf_->
unlock();
425 OpenNiPointCloudThread::fill_xyz_xyzrgb(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
428 pcl_rgb.header.seq += 1;
429 pcl_utils::set_time(pcl_xyzrgb_, ts);
432 pcl_xyz.header.seq += 1;
433 pcl_utils::set_time(pcl_xyz_, ts);
444 unsigned int idx = 0;
445 for (
unsigned int h = 0; h < height_; ++h) {
446 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
448 if (depth_data[idx] == 0 || depth_data[idx] == no_sample_value_
449 || depth_data[idx] == shadow_value_) {
451 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
452 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
454 pclbuf_xyz->
x = pclbuf_xyz->
y = pclbuf_xyz->
z = 0.f;
455 pcl_xyz.points[idx].x = pcl_xyz.points[idx].y = pcl_xyz.points[idx].z = 0.f;
458 pclbuf_rgb->x = pcl_rgb.points[idx].x = pclbuf_xyz->
x = pcl_xyz.points[idx].x =
459 depth_data[idx] * 0.001f;
460 pclbuf_rgb->y = pcl_rgb.points[idx].y = pclbuf_xyz->
y = pcl_xyz.points[idx].y =
461 -(w - center_x_) * depth_data[idx] * foc_const_;
462 pclbuf_rgb->z = pcl_rgb.points[idx].z = pclbuf_xyz->
z = pcl_xyz.points[idx].z =
463 -(h - center_y_) * depth_data[idx] * foc_const_;
470 pcl_xyzrgb_buf_->
unlock();
477 if (!image_rgb_buf_) {
491 for (
unsigned int i = 0; i < width_ * height_; ++i) {
492 pclbuf_rgb->
r = pcl_rgb.points[i].r = imagebuf[i].
R;
493 pclbuf_rgb->
g = pcl_rgb.points[i].g = imagebuf[i].
G;
494 pclbuf_rgb->
b = pcl_rgb.points[i].b = imagebuf[i].
B;
504 bool is_data_new = depth_gen_->IsDataNew();
505 depth_gen_->GetMetaData(*depth_md_);
506 const XnDepthPixel *
const data = depth_md_->Data();
514 || (cfg_generate_pcl_ && ((pcl_xyz_.use_count() > 2)))
517 bool xyzrgb_requested = (pcl_xyzrgb_buf_->
num_attached() > 1)
520 || (cfg_generate_pcl_ && ((pcl_xyzrgb_.use_count() > 2)))
524 if (is_data_new && (xyz_requested || xyzrgb_requested)) {
526 fawkes::Time ts = *capture_start_ + (
long int)depth_gen_->GetTimestamp();
529 if (cfg_generate_pcl_) {
530 if (xyz_requested && xyzrgb_requested) {
531 fill_xyz_xyzrgb(ts, data);
532 }
else if (xyz_requested) {
534 }
else if (xyzrgb_requested) {
535 fill_xyzrgb(ts, data);
540 if (xyz_requested && xyzrgb_requested) {
541 fill_xyz_xyzrgb_no_pcl(ts, data);
542 }
else if (xyz_requested) {
543 fill_xyz_no_pcl(ts, data);
544 }
else if (xyzrgb_requested) {
545 fill_xyzrgb_no_pcl(ts, data);
552 if (!xyzrgb_requested && image_rgb_buf_) {
553 delete image_rgb_buf_;
554 image_rgb_buf_ = NULL;