Fawkes API  Fawkes Development Version
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 <pcl/point_cloud.h>
34 # include <pcl/point_types.h>
35 # include <fvutils/adapters/pcl.h>
36 # include <aspect/pointcloud.h>
37 #endif
38 #include <plugins/openni/aspect/openni.h>
39 
40 // OpenNI relies on GNU extension to detect Linux
41 #if defined(__linux__) && not defined(linux)
42 # define linux true
43 #endif
44 #if defined(__i386__) && not defined(i386)
45 # define i386 true
46 #endif
47 #include <XnCppWrapper.h>
48 
49 #include <map>
50 
51 namespace fawkes {
52  class Time;
53 }
54 
55 namespace firevision {
56  class SharedMemoryImageBuffer;
57 }
58 
59 class OpenNiImageThread;
60 
62 : public fawkes::Thread,
64  public fawkes::LoggingAspect,
66  public fawkes::ClockAspect,
67 #ifdef HAVE_PCL
69 #endif
71 {
72  public:
74  virtual ~OpenNiPointCloudThread();
75 
76  virtual void init();
77  virtual void loop();
78  virtual void finalize();
79 
80  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
81  protected: virtual void run() { Thread::run(); }
82 
83  private:
84  void fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
85  void fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
86  void fill_xyz_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
87  void fill_rgb_no_pcl();
88 
89 #ifdef HAVE_PCL
90  void fill_xyz(fawkes::Time &ts, const XnDepthPixel * const depth_data);
91  void fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data);
92  void fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data);
93  void fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb);
94 #endif
95 
96  private:
97  OpenNiImageThread *__img_thread;
98 
99  xn::DepthGenerator *__depth_gen;
100  xn::ImageGenerator *__image_gen;
101  xn::DepthMetaData *__depth_md;
102 
103  bool __cfg_register_depth_image;
104 
106  firevision::SharedMemoryImageBuffer *__pcl_xyzrgb_buf;
107 
108  firevision::SharedMemoryImageBuffer *__image_rgb_buf;
109 
110  float __focal_length;
111  float __foc_const; // focal length constant used in conversion
112  float __center_x;
113  float __center_y;
114  unsigned int __width;
115  unsigned int __height;
116 
117  XnUInt64 __no_sample_value;
118  XnUInt64 __shadow_value;
119 
120  fawkes::Time *__capture_start;
121 
122  std::string __cfg_frame_depth;
123  std::string __cfg_frame_image;
124 
125 #ifdef HAVE_PCL
126  bool __cfg_generate_pcl;
127 
130 #endif
131 };
132 
133 #endif
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Fawkes library namespace.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
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
Thread aspect to use blocked timing.
Shared memory image buffer.
Definition: shm_image.h:181
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
OpenNI Point Cloud Provider Thread.
OpenNI Image Provider Thread.
Definition: image_thread.h:47