Fawkes API  Fawkes Development Version
tabletop_objects_thread.h
1 
2 /***************************************************************************
3  * tabletop_objects_thread.h - Thread to detect tabletop objects
4  *
5  * Created: Fri Nov 04 23:54:19 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
23 #define __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
24 
25 // must be first for reliable ROS detection
26 #include <pcl/point_cloud.h>
27 #include <pcl/point_types.h>
28 
29 #include <core/threading/thread.h>
30 #include <aspect/clock.h>
31 #include <aspect/configurable.h>
32 #include <aspect/logging.h>
33 #include <aspect/blackboard.h>
34 #include <aspect/tf.h>
35 #include <aspect/pointcloud.h>
36 
37 #include <Eigen/StdVector>
38 #include <pcl/segmentation/sac_segmentation.h>
39 #include <pcl/filters/voxel_grid.h>
40 #include <pcl/features/normal_3d.h>
41 
42 #include <pcl/ModelCoefficients.h>
43 #include <pcl/filters/extract_indices.h>
44 #include <pcl/filters/passthrough.h>
45 #include <pcl/sample_consensus/method_types.h>
46 #include <pcl/sample_consensus/model_types.h>
47 
48 #include <pcl/filters/statistical_outlier_removal.h>
49 
50 #include <map>
51 #include <list>
52 
53 namespace fawkes {
54  class Position3DInterface;
55  class SwitchInterface;
56  class Time;
57 #ifdef USE_TIMETRACKER
58  class TimeTracker;
59 #endif
60 }
61 
62 #ifdef HAVE_VISUAL_DEBUGGING
64 #endif
65 
66 /** @class OldCentroid "tabletop_objects_thread.h"
67  * This class is used to save old centroids in order to check for reappearance
68  */
69 class OldCentroid {
70 public:
71  /**
72  * Constructor
73  * @param id The ID which the centroid was assigned to
74  * @param centroid The position of the centroid
75  */
76  OldCentroid(const unsigned int &id, const Eigen::Vector4f &centroid)
77  : id_(id), age_(0), centroid_(centroid) { }
78  /** Copy constructor
79  * @param other The other OldCentroid */
80  OldCentroid(const OldCentroid &other)
81  : id_(other.getId()), age_(other.getAge()), centroid_(other.getCentroid()) { }
82  virtual ~OldCentroid() { }
83  // any class with Eigen::Vector4f needs a custom new operator
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85  /** get the ID of the centroid
86  * @return the ID of the centroid */
87  unsigned int getId() const { return id_; }
88  /** get the position of the centroid
89  * @return a reference to the centroid */
90  const Eigen::Vector4f& getCentroid() const { return centroid_; }
91  /** get the age of the centroid
92  * @return the number of loops the centroids has been invisible
93  */
94  unsigned int getAge() const { return age_; }
95  /** increment the age of the centroid */
96  void age() { age_++; }
97 
98 protected:
99  /** The ID of the centroid */
100  unsigned int id_;
101  /** The number of loops the centroid has been invisible for */
102  unsigned int age_;
103  /** The position of centroid */
104  Eigen::Vector4f centroid_;
105 };
106 
108 : public fawkes::Thread,
109  public fawkes::ClockAspect,
110  public fawkes::LoggingAspect,
115 {
116  public:
118  virtual ~TabletopObjectsThread();
119 
120  virtual void init();
121  virtual void loop();
122  virtual void finalize();
123 
124 #ifdef HAVE_VISUAL_DEBUGGING
125  void set_visualization_thread(TabletopVisualizationThreadBase *visthread);
126 #endif
127 
128  private:
129  typedef pcl::PointXYZ PointType;
131 
132  typedef pcl::PointXYZRGB ColorPointType;
134  typedef Cloud::Ptr CloudPtr;
135  typedef Cloud::ConstPtr CloudConstPtr;
136 
137  typedef ColorCloud::Ptr ColorCloudPtr;
138  typedef ColorCloud::ConstPtr ColorCloudConstPtr;
139 
140  typedef std::map<unsigned int, Eigen::Vector4f, std::less<unsigned int>,
141  Eigen::aligned_allocator<std::pair<const unsigned int, Eigen::Vector4f>>>
142  CentroidMap;
143  typedef std::list<OldCentroid, Eigen::aligned_allocator<OldCentroid> > OldCentroidVector;
144  typedef std::vector<fawkes::Position3DInterface *> PosIfsVector;
145 
146  private:
147  void set_position(fawkes::Position3DInterface *iface,
148  bool is_visible, const Eigen::Vector4f &centroid = Eigen::Vector4f(0, 0, 0, 0),
149  const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0));
150 
151  CloudPtr simplify_polygon(CloudPtr polygon, float sqr_dist_threshold);
152  CloudPtr generate_table_model(const float length, const float width,
153  const float thickness, const float step, const float max_error);
154  CloudPtr generate_table_model(const float length, const float width,
155  const float step, const float max_error = 0.01);
156  bool is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, PointType &br_p1p, PointType &br_p2p);
157  bool compute_bounding_box_scores(Eigen::Vector3f& cluster_dim, std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >& scores);
158  double compute_similarity(double d1, double d2);
159 
160  void convert_colored_input();
161 
162  std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input);
163 
164  ColorCloudPtr colorize_cluster(CloudConstPtr input_cloud, const std::vector<int> &cluster, const uint8_t color[]);
165 
166  unsigned int cluster_objects(CloudConstPtr input, ColorCloudPtr tmp_clusters, std::vector<ColorCloudPtr> &tmp_obj_clusters);
167 
168  int next_id();
169  void delete_old_centroids(OldCentroidVector centroids, unsigned int age);
170  void delete_near_centroids(CentroidMap reference, OldCentroidVector centroids,
171  float min_distance);
172  void remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids);
173  Eigen::Vector4f fit_cylinder(ColorCloudConstPtr obj_in_base_frame, Eigen::Vector4f const &centroid, uint const &centroid_i);
174  std::map<unsigned int, int> track_objects(std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids);
175 
176  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
177  protected: virtual void run() { Thread::run(); }
178 
179  private:
183  ColorCloudConstPtr colored_input_;
184  CloudPtr converted_input_;
185  CloudConstPtr input_;
187 
188  std::vector<fawkes::RefPtr<pcl::PointCloud<ColorPointType> > > f_obj_clusters_;
189  std::vector<pcl::PointCloud<ColorPointType>::Ptr> obj_clusters_;
190  std::map<unsigned int, double> obj_shape_confidence_;
191 
192 // std::map<unsigned int, std::vector<double>> obj_likelihoods_;
193  std::map<unsigned int, signed int> best_obj_guess_;
194  int NUM_KNOWN_OBJS_;
195 
196  double table_inclination_;
197 
198  std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > known_obj_dimensions_;
199 
200  pcl::VoxelGrid<PointType> grid_;
201  pcl::SACSegmentation<PointType> seg_;
202 
203  PosIfsVector pos_ifs_;
204  fawkes::Position3DInterface *table_pos_if_;
205 
206  Eigen::Vector4f table_centroid;
207 
208  std::list<unsigned int> free_ids_;
209 
210  fawkes::SwitchInterface *switch_if_;
211 
212  fawkes::Time *last_pcl_time_;
213 
214  float cfg_depth_filter_min_x_;
215  float cfg_depth_filter_max_x_;
216  float cfg_voxel_leaf_size_;
217  unsigned int cfg_segm_max_iterations_;
218  float cfg_segm_distance_threshold_;
219  float cfg_segm_inlier_quota_;
220  float cfg_max_z_angle_deviation_;
221  float cfg_table_min_cluster_quota_;
222  float cfg_table_downsample_leaf_size_;
223  float cfg_table_cluster_tolerance_;
224  float cfg_table_min_height_;
225  float cfg_table_max_height_;
226  bool cfg_table_model_enable_;
227  float cfg_table_model_length_;
228  float cfg_table_model_width_;
229  float cfg_table_model_step_;
230  float cfg_horizontal_va_;
231  float cfg_vertical_va_;
232  float cfg_cluster_tolerance_;
233  unsigned int cfg_cluster_min_size_;
234  unsigned int cfg_cluster_max_size_;
235  std::string cfg_base_frame_;
236  std::string cfg_result_frame_;
237  std::string cfg_input_pointcloud_;
238  uint cfg_centroid_max_age_;
239  float cfg_centroid_max_distance_;
240  float cfg_centroid_min_distance_;
241  float cfg_centroid_max_height_;
242  bool cfg_cylinder_fitting_;
243  bool cfg_track_objects_;
244  bool cfg_verbose_cylinder_fitting_;
245 
246  fawkes::RefPtr<Cloud> ftable_model_;
247  CloudPtr table_model_;
248  fawkes::RefPtr<Cloud> fsimplified_polygon_;
249  CloudPtr simplified_polygon_;
250 
251  unsigned int loop_count_;
252 
253  CentroidMap centroids_;
254  CentroidMap cylinder_params_;
255  OldCentroidVector old_centroids_;
256  bool first_run_;
257 
258  std::map<uint, std::vector<double>> obj_likelihoods_;
259 
260 #ifdef USE_TIMETRACKER
261  fawkes::TimeTracker *tt_;
262  unsigned int tt_loopcount_;
263  unsigned int ttc_full_loop_;
264  unsigned int ttc_msgproc_;
265  unsigned int ttc_convert_;
266  unsigned int ttc_voxelize_;
267  unsigned int ttc_plane_;
268  unsigned int ttc_extract_plane_;
269  unsigned int ttc_plane_downsampling_;
270  unsigned int ttc_cluster_plane_;
271  unsigned int ttc_convex_hull_;
272  unsigned int ttc_simplify_polygon_;
273  unsigned int ttc_find_edge_;
274  unsigned int ttc_transform_;
275  unsigned int ttc_transform_model_;
276  unsigned int ttc_extract_non_plane_;
277  unsigned int ttc_polygon_filter_;
278  unsigned int ttc_table_to_output_;
279  unsigned int ttc_cluster_objects_;
280  unsigned int ttc_visualization_;
281  unsigned int ttc_hungarian_;
282  unsigned int ttc_old_centroids_;
283  unsigned int ttc_obj_extraction_;
284 #endif
285 
286 #ifdef HAVE_VISUAL_DEBUGGING
288 #endif
289 };
290 
291 
292 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
virtual void run()
Stub to see name in backtrace for easier debugging.
unsigned int age_
The number of loops the centroid has been invisible for.
Main thread of tabletop objects plugin.
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
EIGEN_MAKE_ALIGNED_OPERATOR_NEW unsigned int getId() const
get the ID of the centroid
A class for handling time.
Definition: time.h:91
Eigen::Vector4f centroid_
The position of centroid.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for virtualization thread.
unsigned int id_
The ID of the centroid.
OldCentroid(const OldCentroid &other)
Copy constructor.
Thread aspect to access the transform system.
Definition: tf.h:42
SwitchInterface Fawkes BlackBoard Interface.
Position3DInterface Fawkes BlackBoard Interface.
Time tracking utility.
Definition: tracker.h:38
Thread aspect to log output.
Definition: logging.h:35
OldCentroid(const unsigned int &id, const Eigen::Vector4f &centroid)
Constructor.
Thread aspect to access configuration data.
Definition: configurable.h:35
unsigned int getAge() const
get the age of the centroid
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
This class is used to save old centroids in order to check for reappearance.
void age()
increment the age of the centroid
const Eigen::Vector4f & getCentroid() const
get the position of the centroid