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> 41 #define FRAME_ID_DEPTH "/kinect/depth" 42 #define FRAME_ID_IMAGE "/kinect/image" 57 :
Thread(
"OpenNiPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
60 __img_thread = img_thread;
75 __image_rgb_buf = NULL;
77 __depth_gen =
new xn::DepthGenerator();
78 std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
79 __image_gen =
new xn::ImageGenerator();
80 std::auto_ptr<xn::ImageGenerator> imagegen_autoptr(__image_gen);
84 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_DEPTH, __depth_gen);
85 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_IMAGE, __image_gen);
86 fawkes::openni::setup_map_generator(*__image_gen,
config);
87 fawkes::openni::setup_map_generator(*__depth_gen,
config);
89 __depth_md =
new xn::DepthMetaData();
90 __depth_gen->GetMetaData(*__depth_md);
92 __cfg_register_depth_image =
false;
94 __cfg_register_depth_image =
config->
get_bool(
"/plugins/openni/register_depth_image");
99 __depth_md->XRes(), __depth_md->YRes());
101 __pcl_xyz_buf->
set_frame_id(__cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH);
105 CARTESIAN_3D_FLOAT_RGB,
106 __depth_md->XRes(), __depth_md->YRes());
108 __pcl_xyzrgb_buf->
set_frame_id(__cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH);
113 if ((st = __depth_gen->GetIntProperty(
"ZPD", zpd)) != XN_STATUS_OK) {
114 throw Exception(
"Failed to get ZPD: %s", xnGetStatusString(st));
117 if ((st = __depth_gen->GetRealProperty(
"ZPPS", pixel_size)) != XN_STATUS_OK) {
118 throw Exception(
"Failed to get ZPPS: %s", xnGetStatusString(st));
121 if ((st = __depth_gen->GetIntProperty(
"NoSampleValue", __no_sample_value))
124 throw Exception(
"Failed to get NoSampleValue: %s", xnGetStatusString(st));
126 if ((st = __depth_gen->GetIntProperty(
"ShadowValue", __shadow_value))
129 throw Exception(
"Failed to get ShadowValue: %s", xnGetStatusString(st));
132 __width = __depth_md->XRes();
133 __height = __depth_md->YRes();
134 float scale = __width / (float)XN_SXGA_X_RES;
135 if (__cfg_register_depth_image) {
137 const float rgb_focal_length_SXGA = 1050;
138 __focal_length = rgb_focal_length_SXGA * scale;
140 __focal_length = ((float)zpd / pixel_size) * scale;
142 __foc_const = 0.001 / __focal_length;
143 __center_x = (__width / 2.) - .5f;
144 __center_y = (__height / 2.) - .5f;
146 __image_gen->StartGenerating();
147 __depth_gen->StartGenerating();
152 __depth_gen->WaitAndUpdateData();
155 *__capture_start -= (
long int)__depth_gen->GetTimestamp();
157 __image_gen->WaitAndUpdateData();
159 if (__cfg_register_depth_image) {
161 fawkes::openni::setup_alternate_viewpoint(*__depth_gen, *__image_gen);
169 __cfg_generate_pcl =
true;
171 __cfg_generate_pcl =
config->
get_bool(
"/plugins/openni-pointcloud/generate-pcl");
174 if (__cfg_generate_pcl) {
176 __pcl_xyz->is_dense =
false;
177 __pcl_xyz->width = __width;
178 __pcl_xyz->height = __height;
179 __pcl_xyz->points.resize(__width * __height);
180 __pcl_xyz->header.frame_id = __cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH;
183 __pcl_xyzrgb->is_dense =
false;
184 __pcl_xyzrgb->width = __width;
185 __pcl_xyzrgb->height = __height;
186 __pcl_xyzrgb->points.resize(__width * __height);
187 __pcl_xyzrgb->header.frame_id = __cfg_register_depth_image ? FRAME_ID_IMAGE : FRAME_ID_DEPTH;
189 pcl_manager->add_pointcloud(
"openni-pointcloud-xyz", __pcl_xyz);
190 pcl_manager->add_pointcloud(
"openni-pointcloud-xyzrgb", __pcl_xyzrgb);
194 depthgen_autoptr.release();
195 imagegen_autoptr.release();
203 pcl_manager->remove_pointcloud(
"openni-pointcloud-xyz");
204 pcl_manager->remove_pointcloud(
"openni-pointcloud-xyzrgb");
211 delete __pcl_xyz_buf;
212 delete __pcl_xyzrgb_buf;
213 delete __capture_start;
218 OpenNiPointCloudThread::fill_xyz_no_pcl(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
225 unsigned int idx = 0;
226 for (
unsigned int h = 0; h < __height; ++h) {
227 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
229 if (depth_data[idx] == 0 ||
230 depth_data[idx] == __no_sample_value ||
231 depth_data[idx] == __shadow_value)
234 pclbuf->
x = pclbuf->
y = pclbuf->
z = 0.f;
237 pclbuf->
x = depth_data[idx] * 0.001f;
238 pclbuf->
y = -(w - __center_x) * depth_data[idx] * __foc_const;
239 pclbuf->
z = -(h - __center_y) * depth_data[idx] * __foc_const;
249 OpenNiPointCloudThread::fill_xyzrgb_no_pcl(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
256 unsigned int idx = 0;
257 for (
unsigned int h = 0; h < __height; ++h) {
258 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
260 if (depth_data[idx] == 0 ||
261 depth_data[idx] == __no_sample_value ||
262 depth_data[idx] == __shadow_value)
265 pclbuf_rgb->
x = pclbuf_rgb->
y = pclbuf_rgb->
z = 0.f;
268 pclbuf_rgb->
x = depth_data[idx] * 0.001f;
269 pclbuf_rgb->
y = -(w - __center_x) * depth_data[idx] * __foc_const;
270 pclbuf_rgb->
z = -(h - __center_y) * depth_data[idx] * __foc_const;
277 __pcl_xyzrgb_buf->
unlock();
282 OpenNiPointCloudThread::fill_xyz_xyzrgb_no_pcl(
fawkes::Time &ts,
283 const XnDepthPixel *
const depth_data)
294 unsigned int idx = 0;
295 for (
unsigned int h = 0; h < __height; ++h) {
296 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
298 if (depth_data[idx] == 0 ||
299 depth_data[idx] == __no_sample_value ||
300 depth_data[idx] == __shadow_value)
303 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
304 pclbuf_xyz->
x = pclbuf_xyz->
y = pclbuf_xyz->
z = 0.f;
307 pclbuf_rgb->x = pclbuf_xyz->
x = depth_data[idx] * 0.001f;
308 pclbuf_rgb->y = pclbuf_xyz->
y = -(w - __center_x) * depth_data[idx] * __foc_const;
309 pclbuf_rgb->z = pclbuf_xyz->
z = -(h - __center_y) * depth_data[idx] * __foc_const;
316 __pcl_xyzrgb_buf->
unlock();
322 OpenNiPointCloudThread::fill_rgb_no_pcl()
324 if (! __image_rgb_buf) {
338 for (
unsigned int i = 0; i < __width * __height; ++i) {
339 pclbuf_rgb->
r = imagebuf[i].
R;
340 pclbuf_rgb->
g = imagebuf[i].
G;
341 pclbuf_rgb->
b = imagebuf[i].
B;
349 OpenNiPointCloudThread::fill_xyz(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
353 pcl_utils::set_time(__pcl_xyz, ts);
360 unsigned int idx = 0;
361 for (
unsigned int h = 0; h < __height; ++h) {
362 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
364 if (depth_data[idx] == 0 ||
365 depth_data[idx] == __no_sample_value ||
366 depth_data[idx] == __shadow_value)
369 pclbuf->
x = pclbuf->
y = pclbuf->
z = 0.f;
370 pcl.points[idx].x = pcl.points[idx].y = pcl.points[idx].z = 0.f;
373 pclbuf->
x = pcl.points[idx].x = depth_data[idx] * 0.001f;
374 pclbuf->
y = pcl.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
375 pclbuf->
z = pcl.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
385 OpenNiPointCloudThread::fill_xyzrgb(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
388 pcl_rgb.header.seq += 1;
389 pcl_utils::set_time(__pcl_xyzrgb, ts);
396 unsigned int idx = 0;
397 for (
unsigned int h = 0; h < __height; ++h) {
398 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb) {
400 if (depth_data[idx] == 0 ||
401 depth_data[idx] == __no_sample_value ||
402 depth_data[idx] == __shadow_value)
405 pclbuf_rgb->
x = pclbuf_rgb->
y = pclbuf_rgb->
z = 0.f;
406 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
409 pclbuf_rgb->
x = pcl_rgb.points[idx].x = depth_data[idx] * 0.001f;
410 pclbuf_rgb->
y = pcl_rgb.points[idx].y = -(w - __center_x) * depth_data[idx] * __foc_const;
411 pclbuf_rgb->
z = pcl_rgb.points[idx].z = -(h - __center_y) * depth_data[idx] * __foc_const;
418 __pcl_xyzrgb_buf->
unlock();
423 OpenNiPointCloudThread::fill_xyz_xyzrgb(
fawkes::Time &ts,
const XnDepthPixel *
const depth_data)
427 pcl_rgb.header.seq += 1;
428 pcl_utils::set_time(__pcl_xyzrgb, ts);
431 pcl_xyz.header.seq += 1;
432 pcl_utils::set_time(__pcl_xyz, ts);
443 unsigned int idx = 0;
444 for (
unsigned int h = 0; h < __height; ++h) {
445 for (
unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf_rgb, ++pclbuf_xyz) {
447 if (depth_data[idx] == 0 ||
448 depth_data[idx] == __no_sample_value ||
449 depth_data[idx] == __shadow_value)
452 pclbuf_rgb->x = pclbuf_rgb->y = pclbuf_rgb->z = 0.f;
453 pcl_rgb.points[idx].x = pcl_rgb.points[idx].y = pcl_rgb.points[idx].z = 0.f;
455 pclbuf_xyz->
x = pclbuf_xyz->
y = pclbuf_xyz->
z = 0.f;
456 pcl_xyz.points[idx].x = pcl_xyz.points[idx].y = pcl_xyz.points[idx].z = 0.f;
459 pclbuf_rgb->x = pcl_rgb.points[idx].x = pclbuf_xyz->
x = pcl_xyz.points[idx].x =
460 depth_data[idx] * 0.001f;
461 pclbuf_rgb->y = pcl_rgb.points[idx].y = pclbuf_xyz->
y = pcl_xyz.points[idx].y =
462 -(w - __center_x) * depth_data[idx] * __foc_const;
463 pclbuf_rgb->z = pcl_rgb.points[idx].z = pclbuf_xyz->
z = pcl_xyz.points[idx].z =
464 -(h - __center_y) * depth_data[idx] * __foc_const;
471 __pcl_xyzrgb_buf->
unlock();
479 if (! __image_rgb_buf) {
493 for (
unsigned int i = 0; i < __width * __height; ++i) {
494 pclbuf_rgb->
r = pcl_rgb.points[i].r = imagebuf[i].
R;
495 pclbuf_rgb->
g = pcl_rgb.points[i].g = imagebuf[i].
G;
496 pclbuf_rgb->
b = pcl_rgb.points[i].b = imagebuf[i].
B;
507 bool is_data_new = __depth_gen->IsDataNew();
508 __depth_gen->GetMetaData(*__depth_md);
509 const XnDepthPixel *
const data = __depth_md->Data();
514 bool xyz_requested = (__pcl_xyz_buf->
num_attached() > 1)
517 || (__cfg_generate_pcl && ((__pcl_xyz.use_count() > 2)))
520 bool xyzrgb_requested = (__pcl_xyzrgb_buf->
num_attached() > 1)
523 || (__cfg_generate_pcl && ((__pcl_xyzrgb.use_count() > 2)))
527 if (is_data_new && (xyz_requested || xyzrgb_requested)) {
529 fawkes::Time ts = *__capture_start + (
long int)__depth_gen->GetTimestamp();
532 if (__cfg_generate_pcl) {
534 if (xyz_requested && xyzrgb_requested) {
535 fill_xyz_xyzrgb(ts, data);
536 }
else if (xyz_requested) {
538 }
else if (xyzrgb_requested) {
539 fill_xyzrgb(ts, data);
544 if (xyz_requested && xyzrgb_requested) {
545 fill_xyz_xyzrgb_no_pcl(ts, data);
546 }
else if (xyz_requested) {
547 fill_xyz_no_pcl(ts, data);
548 }
else if (xyzrgb_requested) {
549 fill_xyzrgb_no_pcl(ts, data);
556 if (! xyzrgb_requested && __image_rgb_buf) {
557 delete __image_rgb_buf;
558 __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.
Mutex * objmutex_ptr() const
Get object mutex.
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.
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.
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 char * buffer() const
Get image buffer.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
unsigned int num_attached() const
Get number of attached processes.
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.