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 <aspect/blocked_timing.h>
27 #include <aspect/clock.h>
28 #include <aspect/configurable.h>
29 #include <aspect/logging.h>
30 #include <core/threading/thread.h>
31 #include <core/utils/lockptr.h>
32 #ifdef HAVE_PCL
33 # include <aspect/pointcloud.h>
34 # include <fvutils/adapters/pcl.h>
35 # include <pcl/point_cloud.h>
36 # include <pcl/point_types.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 #include <map>
49 
50 namespace fawkes {
51 class Time;
52 }
53 
54 namespace firevision {
55 class SharedMemoryImageBuffer;
56 }
57 
58 class OpenNiImageThread;
59 
62  public fawkes::LoggingAspect,
64  public fawkes::ClockAspect,
65 #ifdef HAVE_PCL
67 #endif
69 {
70 public:
72  virtual ~OpenNiPointCloudThread();
73 
74  virtual void init();
75  virtual void loop();
76  virtual void finalize();
77 
78  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
79 protected:
80  virtual void
81  run()
82  {
83  Thread::run();
84  }
85 
86 private:
87  void fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel *const data);
88  void fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel *const data);
89  void fill_xyz_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel *const data);
90  void fill_rgb_no_pcl();
91 
92 #ifdef HAVE_PCL
93  void fill_xyz(fawkes::Time &ts, const XnDepthPixel *const depth_data);
94  void fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel *const depth_data);
95  void fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel *const depth_data);
96  void fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb);
97 #endif
98 
99 private:
100  OpenNiImageThread *img_thread_;
101 
102  xn::DepthGenerator *depth_gen_;
103  xn::ImageGenerator *image_gen_;
104  xn::DepthMetaData * depth_md_;
105 
106  bool cfg_register_depth_image_;
107 
109  firevision::SharedMemoryImageBuffer *pcl_xyzrgb_buf_;
110 
111  firevision::SharedMemoryImageBuffer *image_rgb_buf_;
112 
113  float focal_length_;
114  float foc_const_; // focal length constant used in conversion
115  float center_x_;
116  float center_y_;
117  unsigned int width_;
118  unsigned int height_;
119 
120  XnUInt64 no_sample_value_;
121  XnUInt64 shadow_value_;
122 
123  fawkes::Time *capture_start_;
124 
125  std::string cfg_frame_depth_;
126  std::string cfg_frame_image_;
127 
128 #ifdef HAVE_PCL
129  bool cfg_generate_pcl_;
130 
133 #endif
134 };
135 
136 #endif
OpenNI Image Provider Thread.
Definition: image_thread.h:53
OpenNI Point Cloud Provider Thread.
OpenNiPointCloudThread(OpenNiImageThread *img_thread)
Constructor.
virtual void init()
Initialize the thread.
virtual ~OpenNiPointCloudThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to get access to the OpenNI context.
Definition: openni.h:39
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Shared memory image buffer.
Definition: shm_image.h:184
Fawkes library namespace.