Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
pointcloud_thread.h
1 
2 /***************************************************************************
3  * pointcloud_thread.h - OpenNI point cloud provider thread
4  *
5  * Created: Fri Mar 25 23:48:05 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef __PLUGINS_OPENNI_POINTCLOUD_THREAD_H_
24 #define __PLUGINS_OPENNI_POINTCLOUD_THREAD_H_
25 
26 #include <core/threading/thread.h>
27 #include <core/utils/lockptr.h>
28 #include <aspect/logging.h>
29 #include <aspect/configurable.h>
30 #include <aspect/clock.h>
31 #include <aspect/blocked_timing.h>
32 #ifdef HAVE_PCL
33 # include <aspect/pointcloud.h>
34 # include <pcl/point_cloud.h>
35 # include <pcl/point_types.h>
36 # include <fvutils/adapters/pcl.h>
37 #endif
38 #include <plugins/openni/aspect/openni.h>
39 
40 #include <XnCppWrapper.h>
41 
42 #include <map>
43 
44 namespace fawkes {
45  class Time;
46 }
47 
48 namespace firevision {
49  class SharedMemoryImageBuffer;
50 }
51 
52 class OpenNiImageThread;
53 
55 : public fawkes::Thread,
57  public fawkes::LoggingAspect,
59  public fawkes::ClockAspect,
60 #ifdef HAVE_PCL
62 #endif
64 {
65  public:
67  virtual ~OpenNiPointCloudThread();
68 
69  virtual void init();
70  virtual void loop();
71  virtual void finalize();
72 
73  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
74  protected: virtual void run() { Thread::run(); }
75 
76  private:
77  void fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
78  void fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
79  void fill_xyz_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
80  void fill_rgb_no_pcl();
81 
82 #ifdef HAVE_PCL
83  void fill_xyz(fawkes::Time &ts, const XnDepthPixel * const depth_data);
84  void fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data);
85  void fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data);
86  void fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb);
87 #endif
88 
89  private:
90  OpenNiImageThread *__img_thread;
91 
92  xn::DepthGenerator *__depth_gen;
93  xn::ImageGenerator *__image_gen;
94  xn::DepthMetaData *__depth_md;
95 
96  bool __cfg_register_depth_image;
97 
99  firevision::SharedMemoryImageBuffer *__pcl_xyzrgb_buf;
100 
101  firevision::SharedMemoryImageBuffer *__image_rgb_buf;
102 
103  float __focal_length;
104  float __foc_const; // focal length constant used in conversion
105  float __center_x;
106  float __center_y;
107  unsigned int __width;
108  unsigned int __height;
109 
110  XnUInt64 __no_sample_value;
111  XnUInt64 __shadow_value;
112 
113  fawkes::Time *__capture_start;
114 
115 #ifdef HAVE_PCL
116  bool __cfg_generate_pcl;
117 
120 #endif
121 };
122 
123 #endif
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
Thread aspect to get access to the OpenNI context.
Definition: openni.h:39
virtual void run()
Stub to see name in backtrace for easier debugging.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
virtual void finalize()
Finalize the thread.
virtual ~OpenNiPointCloudThread()
Destructor.
Shared memory image buffer.
Definition: shm_image.h:135
Thread aspect to log output.
Definition: logging.h:35
virtual void loop()
Code to execute in the thread.
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr&lt;&gt; is a reference-counting shared smartpointer.
Definition: refptr.h:49
OpenNI Point Cloud Provider Thread.
OpenNI Image Provider Thread.
Definition: image_thread.h:40
OpenNiPointCloudThread(OpenNiImageThread *img_thread)
Constructor.