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
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.
virtual ~TabletopObjectsThread()
Destructor.
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.
virtual void finalize()
Finalize the thread.
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
virtual void init()
Initialize the thread.
This class is used to save old centroids in order to check for reappearance.
virtual void loop()
Code to execute in the thread.
void age()
increment the age of the centroid
const Eigen::Vector4f & getCentroid() const
get the position of the centroid