Fawkes API  Fawkes Development Version
pointcloud_thread.h
00001 
00002 /***************************************************************************
00003  *  pointcloud_thread.h - OpenNI point cloud provider thread
00004  *
00005  *  Created: Fri Mar 25 23:48:05 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #ifndef __PLUGINS_OPENNI_POINTCLOUD_THREAD_H_
00024 #define __PLUGINS_OPENNI_POINTCLOUD_THREAD_H_
00025 
00026 #include <core/threading/thread.h>
00027 #include <core/utils/lockptr.h>
00028 #include <aspect/logging.h>
00029 #include <aspect/configurable.h>
00030 #include <aspect/clock.h>
00031 #include <aspect/blocked_timing.h>
00032 #ifdef HAVE_PCL
00033 #  include <aspect/pointcloud.h>
00034 #  include <pcl/point_cloud.h>
00035 #  include <pcl/point_types.h>
00036 #  include <fvutils/adapters/pcl.h>
00037 #endif
00038 #include <plugins/openni/aspect/openni.h>
00039 
00040 #include <XnCppWrapper.h>
00041 
00042 #include <map>
00043 
00044 namespace fawkes {
00045   class Time;
00046 }
00047 
00048 namespace firevision {
00049   class SharedMemoryImageBuffer;
00050 }
00051 
00052 class OpenNiImageThread;
00053 
00054 class OpenNiPointCloudThread
00055 : public fawkes::Thread,
00056   public fawkes::BlockedTimingAspect,
00057   public fawkes::LoggingAspect,
00058   public fawkes::ConfigurableAspect,
00059   public fawkes::ClockAspect,
00060 #ifdef HAVE_PCL
00061   public fawkes::PointCloudAspect,
00062 #endif
00063   public fawkes::OpenNiAspect
00064 {
00065  public:
00066   OpenNiPointCloudThread(OpenNiImageThread *img_thread);
00067   virtual ~OpenNiPointCloudThread();
00068 
00069   virtual void init();
00070   virtual void loop();
00071   virtual void finalize();
00072 
00073  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00074  protected: virtual void run() { Thread::run(); }
00075 
00076  private:
00077   void fill_xyz_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
00078   void fill_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
00079   void fill_xyz_xyzrgb_no_pcl(fawkes::Time &ts, const XnDepthPixel * const data);
00080   void fill_rgb_no_pcl();
00081 
00082 #ifdef HAVE_PCL
00083   void fill_xyz(fawkes::Time &ts, const XnDepthPixel * const depth_data);
00084   void fill_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data);
00085   void fill_xyz_xyzrgb(fawkes::Time &ts, const XnDepthPixel * const depth_data);
00086   void fill_rgb(pcl::PointCloud<pcl::PointXYZRGB> &pcl_rgb);
00087 #endif
00088 
00089  private:
00090   OpenNiImageThread *__img_thread;
00091 
00092   xn::DepthGenerator  *__depth_gen;
00093   xn::ImageGenerator  *__image_gen;
00094   xn::DepthMetaData   *__depth_md;
00095 
00096   bool         __cfg_register_depth_image;
00097 
00098   firevision::SharedMemoryImageBuffer *__pcl_xyz_buf;
00099   firevision::SharedMemoryImageBuffer *__pcl_xyzrgb_buf;
00100 
00101   firevision::SharedMemoryImageBuffer *__image_rgb_buf;
00102 
00103   float        __focal_length;
00104   float        __foc_const; // focal length constant used in conversion
00105   float        __center_x;
00106   float        __center_y;
00107   unsigned int __width;
00108   unsigned int __height;
00109 
00110   XnUInt64     __no_sample_value;
00111   XnUInt64     __shadow_value;
00112 
00113   fawkes::Time *__capture_start;
00114 
00115 #ifdef HAVE_PCL
00116   bool         __cfg_generate_pcl;
00117 
00118   fawkes::RefPtr<pcl::PointCloud<pcl::PointXYZ> >    __pcl_xyz;
00119   fawkes::RefPtr<pcl::PointCloud<pcl::PointXYZRGB> > __pcl_xyzrgb;
00120 #endif
00121 };
00122 
00123 #endif