Fawkes API  Fawkes Development Version
tabletop_objects_thread.h
00001 
00002 /***************************************************************************
00003  *  tabletop_objects_thread.h - Thread to detect tabletop objects
00004  *
00005  *  Created: Fri Nov 04 23:54:19 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #ifndef __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
00023 #define __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
00024 
00025 #include <core/threading/thread.h>
00026 #include <aspect/clock.h>
00027 #include <aspect/configurable.h>
00028 #include <aspect/logging.h>
00029 #include <aspect/blackboard.h>
00030 #include <aspect/tf.h>
00031 #include <aspect/pointcloud.h>
00032 
00033 #include <Eigen/StdVector>
00034 #include <pcl/point_types.h>
00035 #include <pcl/point_cloud.h>
00036 #include <pcl/segmentation/sac_segmentation.h>
00037 #include <pcl/filters/voxel_grid.h>
00038 
00039 namespace fawkes {
00040   class Position3DInterface;
00041   class SwitchInterface;
00042 #ifdef USE_TIMETRACKER
00043   class TimeTracker;
00044 #endif
00045 }
00046 
00047 #ifdef HAVE_VISUAL_DEBUGGING
00048 class TabletopVisualizationThreadBase;
00049 #endif
00050 
00051 class TabletopObjectsThread
00052 : public fawkes::Thread,
00053   public fawkes::ClockAspect,
00054   public fawkes::LoggingAspect,
00055   public fawkes::ConfigurableAspect,
00056   public fawkes::BlackBoardAspect,
00057   public fawkes::TransformAspect,
00058   public fawkes::PointCloudAspect
00059 {
00060  public:
00061   TabletopObjectsThread();
00062   virtual ~TabletopObjectsThread();
00063 
00064   virtual void init();
00065   virtual void loop();
00066   virtual void finalize();
00067 
00068 #ifdef HAVE_VISUAL_DEBUGGING
00069   void set_visualization_thread(TabletopVisualizationThreadBase *visthread);
00070 #endif
00071 
00072  private:
00073   typedef pcl::PointXYZ PointType;
00074   typedef pcl::PointCloud<PointType> Cloud;
00075 
00076   typedef pcl::PointXYZRGB ColorPointType;
00077   typedef pcl::PointCloud<ColorPointType> ColorCloud;
00078   typedef Cloud::Ptr CloudPtr;
00079   typedef Cloud::ConstPtr CloudConstPtr;
00080 
00081   typedef ColorCloud::Ptr ColorCloudPtr;
00082   typedef ColorCloud::ConstPtr ColorCloudConstPtr;
00083 
00084  private:
00085   void set_position(fawkes::Position3DInterface *iface,
00086                     bool is_visible, const Eigen::Vector4f &centroid = Eigen::Vector4f(0, 0, 0, 0),
00087                     const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0));
00088 
00089   CloudPtr simplify_polygon(CloudPtr polygon, float sqr_dist_threshold);
00090 
00091   CloudPtr generate_table_model(const float length, const float width,
00092                                 const float thickness, const float step, const float max_error);
00093 
00094   CloudPtr generate_table_model(const float length, const float width,
00095                                 const float step, const float max_error = 0.01);
00096 
00097   bool is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, PointType &br_p1p, PointType &br_p2p);
00098 
00099  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00100  protected: virtual void run() { Thread::run(); }
00101 
00102  private:
00103   fawkes::RefPtr<const pcl::PointCloud<PointType> > finput_;
00104   fawkes::RefPtr<pcl::PointCloud<ColorPointType> > fclusters_;
00105   CloudConstPtr input_;
00106   pcl::PointCloud<ColorPointType>::Ptr clusters_;
00107 
00108   pcl::VoxelGrid<PointType> grid_;
00109   pcl::SACSegmentation<PointType> seg_;
00110 
00111   std::vector<fawkes::Position3DInterface *> pos_ifs_;
00112   fawkes::Position3DInterface *table_pos_if_;
00113 
00114   fawkes::SwitchInterface *switch_if_;
00115 
00116   float cfg_depth_filter_min_x_;
00117   float cfg_depth_filter_max_x_;
00118   float cfg_voxel_leaf_size_;
00119   unsigned int cfg_segm_max_iterations_;
00120   float cfg_segm_distance_threshold_;
00121   float cfg_segm_inlier_quota_;
00122   float cfg_max_z_angle_deviation_;
00123   float cfg_table_min_height_;
00124   float cfg_table_max_height_;
00125   float cfg_table_model_length_;
00126   float cfg_table_model_width_;
00127   float cfg_table_model_step_;
00128   float cfg_horizontal_va_;
00129   float cfg_vertical_va_;
00130   float cfg_cluster_tolerance_;
00131   unsigned int cfg_cluster_min_size_;
00132   unsigned int cfg_cluster_max_size_;
00133   std::string cfg_result_frame_;
00134 
00135   fawkes::RefPtr<Cloud> ftable_model_;
00136   CloudPtr table_model_;
00137   fawkes::RefPtr<Cloud> fsimplified_polygon_;
00138   CloudPtr simplified_polygon_;
00139 
00140   unsigned int loop_count_;
00141 
00142 #ifdef USE_TIMETRACKER
00143   fawkes::TimeTracker  *tt_;
00144   unsigned int tt_loopcount_;
00145   unsigned int ttc_full_loop_;
00146   unsigned int ttc_msgproc_;
00147   unsigned int ttc_voxelize_;
00148   unsigned int ttc_plane_;
00149   unsigned int ttc_extract_plane_;
00150   unsigned int ttc_plane_downsampling_;
00151   unsigned int ttc_cluster_plane_;
00152   unsigned int ttc_convex_hull_;
00153   unsigned int ttc_simplify_polygon_;
00154   unsigned int ttc_find_edge_;
00155   unsigned int ttc_transform_;
00156   unsigned int ttc_transform_model_;
00157   unsigned int ttc_extract_non_plane_;
00158   unsigned int ttc_polygon_filter_;
00159   unsigned int ttc_table_to_output_;
00160   unsigned int ttc_cluster_objects_;
00161   unsigned int ttc_visualization_;
00162 #endif
00163 
00164 #ifdef HAVE_VISUAL_DEBUGGING
00165   TabletopVisualizationThreadBase *visthread_;
00166 #endif
00167 };
00168 
00169 #endif