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 <aspect/blackboard.h>
27 #include <aspect/clock.h>
28 #include <aspect/configurable.h>
29 #include <aspect/logging.h>
30 #include <aspect/pointcloud.h>
31 #include <aspect/tf.h>
32 #include <core/threading/thread.h>
33 #include <pcl/ModelCoefficients.h>
34 #include <pcl/features/normal_3d.h>
35 #include <pcl/filters/extract_indices.h>
36 #include <pcl/filters/passthrough.h>
37 #include <pcl/filters/statistical_outlier_removal.h>
38 #include <pcl/filters/voxel_grid.h>
39 #include <pcl/point_cloud.h>
40 #include <pcl/point_types.h>
41 #include <pcl/sample_consensus/method_types.h>
42 #include <pcl/sample_consensus/model_types.h>
43 #include <pcl/segmentation/sac_segmentation.h>
44 
45 #include <Eigen/StdVector>
46 #include <list>
47 #include <map>
48 
49 namespace fawkes {
50 class Position3DInterface;
51 class SwitchInterface;
52 class Time;
53 #ifdef USE_TIMETRACKER
54 class TimeTracker;
55 #endif
56 } // namespace fawkes
57 
58 #ifdef HAVE_VISUAL_DEBUGGING
60 #endif
61 
62 /** @class OldCentroid "tabletop_objects_thread.h"
63  * This class is used to save old centroids in order to check for reappearance.
64  */
66 {
67 public:
68  /**
69  * Constructor
70  * @param id The ID which the centroid was assigned to
71  * @param centroid The position of the centroid
72  */
73  OldCentroid(const unsigned int &id, const Eigen::Vector4f &centroid)
74  : id_(id), age_(0), centroid_(centroid)
75  {
76  }
77 
78  /** Copy constructor
79  * @param other The other OldCentroid */
80  OldCentroid(const OldCentroid &other)
81  : id_(other.get_id()), age_(other.get_age()), centroid_(other.get_centroid())
82  {
83  }
84 
85  /** Destructor. */
86  virtual ~OldCentroid()
87  {
88  }
89 
90  /** Assignment operator.
91  * @param other The other OldCentroid
92  * @return reference to this instance */
93  OldCentroid &
94  operator=(const OldCentroid &other)
95  {
96  id_ = other.get_id();
97  age_ = other.get_age();
98  centroid_ = other.get_centroid();
99  return *this;
100  }
101 
102  // any class with Eigen::Vector4f needs a custom new operator
103  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
104 
105  /** Get the ID of the centroid
106  * @return the ID of the centroid */
107  unsigned int
108  get_id() const
109  {
110  return id_;
111  }
112 
113  /** Get the position of the centroid
114  * @return a reference to the centroid */
115  const Eigen::Vector4f &
116  get_centroid() const
117  {
118  return centroid_;
119  }
120 
121  /** Get the age of the centroid
122  * @return the number of loops the centroids has been invisible
123  */
124  unsigned int
125  get_age() const
126  {
127  return age_;
128  }
129 
130  /** Increment the age of the centroid */
131  void
133  {
134  age_ += 1;
135  }
136 
137 protected:
138  /** The ID of the centroid */
139  unsigned int id_;
140  /** The number of loops the centroid has been invisible for */
141  unsigned int age_;
142  /** The position of centroid */
143  Eigen::Vector4f centroid_;
144 };
145 
147  public fawkes::ClockAspect,
148  public fawkes::LoggingAspect,
153 {
154 public:
156  virtual ~TabletopObjectsThread();
157 
158  virtual void init();
159  virtual void loop();
160  virtual void finalize();
161 
162 #ifdef HAVE_VISUAL_DEBUGGING
163  void set_visualization_thread(TabletopVisualizationThreadBase *visthread);
164 #endif
165 
166 private:
167  typedef pcl::PointXYZ PointType;
169 
170  typedef pcl::PointXYZRGB ColorPointType;
172  typedef Cloud::Ptr CloudPtr;
173  typedef Cloud::ConstPtr CloudConstPtr;
174 
175  typedef ColorCloud::Ptr ColorCloudPtr;
176  typedef ColorCloud::ConstPtr ColorCloudConstPtr;
177 
178  typedef std::map<unsigned int,
179  Eigen::Vector4f,
180  std::less<unsigned int>,
181  Eigen::aligned_allocator<std::pair<const unsigned int, Eigen::Vector4f>>>
182  CentroidMap;
183  typedef std::list<OldCentroid, Eigen::aligned_allocator<OldCentroid>> OldCentroidVector;
184  typedef std::vector<fawkes::Position3DInterface *> PosIfsVector;
185 
186 private:
187  void set_position(fawkes::Position3DInterface *iface,
188  bool is_visible,
189  const Eigen::Vector4f & centroid = Eigen::Vector4f(0, 0, 0, 0),
190  const Eigen::Quaternionf & rotation = Eigen::Quaternionf(1, 0, 0, 0));
191 
192  CloudPtr simplify_polygon(CloudPtr polygon, float sqr_dist_threshold);
193  CloudPtr generate_table_model(const float length,
194  const float width,
195  const float thickness,
196  const float step,
197  const float max_error);
198  CloudPtr generate_table_model(const float length,
199  const float width,
200  const float step,
201  const float max_error = 0.01);
202  bool is_polygon_edge_better(PointType &cb_br_p1p,
203  PointType &cb_br_p2p,
204  PointType &br_p1p,
205  PointType &br_p2p);
206  bool compute_bounding_box_scores(
207  Eigen::Vector3f & cluster_dim,
208  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> &scores);
209  double compute_similarity(double d1, double d2);
210 
211  void convert_colored_input();
212 
213  std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input);
214 
215  ColorCloudPtr colorize_cluster(CloudConstPtr input_cloud,
216  const std::vector<int> &cluster,
217  const uint8_t color[]);
218 
219  unsigned int cluster_objects(CloudConstPtr input,
220  ColorCloudPtr tmp_clusters,
221  std::vector<ColorCloudPtr> &tmp_obj_clusters);
222 
223  int next_id();
224  void delete_old_centroids(OldCentroidVector centroids, unsigned int age);
225  void
226  delete_near_centroids(CentroidMap reference, OldCentroidVector centroids, float min_distance);
227  void remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids);
228  Eigen::Vector4f fit_cylinder(ColorCloudConstPtr obj_in_base_frame,
229  Eigen::Vector4f const &centroid,
230  uint const & centroid_i);
231  std::map<unsigned int, int> track_objects(
232  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids);
233 
234  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
235 protected:
236  virtual void
237  run()
238  {
239  Thread::run();
240  }
241 
242 private:
246  ColorCloudConstPtr colored_input_;
247  CloudPtr converted_input_;
248  CloudConstPtr input_;
250 
251  std::vector<fawkes::RefPtr<pcl::PointCloud<ColorPointType>>> f_obj_clusters_;
252  std::vector<pcl::PointCloud<ColorPointType>::Ptr> obj_clusters_;
253  std::map<unsigned int, double> obj_shape_confidence_;
254 
255  // std::map<unsigned int, std::vector<double>> obj_likelihoods_;
256  std::map<unsigned int, signed int> best_obj_guess_;
257  int NUM_KNOWN_OBJS_;
258 
259  double table_inclination_;
260 
261  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> known_obj_dimensions_;
262 
263  pcl::VoxelGrid<PointType> grid_;
264  pcl::SACSegmentation<PointType> seg_;
265 
266  PosIfsVector pos_ifs_;
267  fawkes::Position3DInterface *table_pos_if_;
268 
269  Eigen::Vector4f table_centroid;
270 
271  std::list<unsigned int> free_ids_;
272 
273  fawkes::SwitchInterface *switch_if_;
274 
275  fawkes::Time *last_pcl_time_;
276 
277  float cfg_depth_filter_min_x_;
278  float cfg_depth_filter_max_x_;
279  float cfg_voxel_leaf_size_;
280  unsigned int cfg_segm_max_iterations_;
281  float cfg_segm_distance_threshold_;
282  float cfg_segm_inlier_quota_;
283  float cfg_max_z_angle_deviation_;
284  float cfg_table_min_cluster_quota_;
285  float cfg_table_downsample_leaf_size_;
286  float cfg_table_cluster_tolerance_;
287  float cfg_table_min_height_;
288  float cfg_table_max_height_;
289  bool cfg_table_model_enable_;
290  float cfg_table_model_length_;
291  float cfg_table_model_width_;
292  float cfg_table_model_step_;
293  float cfg_horizontal_va_;
294  float cfg_vertical_va_;
295  float cfg_cluster_tolerance_;
296  unsigned int cfg_cluster_min_size_;
297  unsigned int cfg_cluster_max_size_;
298  std::string cfg_base_frame_;
299  std::string cfg_result_frame_;
300  std::string cfg_input_pointcloud_;
301  uint cfg_centroid_max_age_;
302  float cfg_centroid_max_distance_;
303  float cfg_centroid_min_distance_;
304  float cfg_centroid_max_height_;
305  bool cfg_cylinder_fitting_;
306  bool cfg_track_objects_;
307  bool cfg_verbose_cylinder_fitting_;
308 
309  fawkes::RefPtr<Cloud> ftable_model_;
310  CloudPtr table_model_;
311  fawkes::RefPtr<Cloud> fsimplified_polygon_;
312  CloudPtr simplified_polygon_;
313 
314  unsigned int loop_count_;
315 
316  CentroidMap centroids_;
317  CentroidMap cylinder_params_;
318  OldCentroidVector old_centroids_;
319  bool first_run_;
320 
321  std::map<uint, std::vector<double>> obj_likelihoods_;
322 
323 #ifdef USE_TIMETRACKER
324  fawkes::TimeTracker *tt_;
325  unsigned int tt_loopcount_;
326  unsigned int ttc_full_loop_;
327  unsigned int ttc_msgproc_;
328  unsigned int ttc_convert_;
329  unsigned int ttc_voxelize_;
330  unsigned int ttc_plane_;
331  unsigned int ttc_extract_plane_;
332  unsigned int ttc_plane_downsampling_;
333  unsigned int ttc_cluster_plane_;
334  unsigned int ttc_convex_hull_;
335  unsigned int ttc_simplify_polygon_;
336  unsigned int ttc_find_edge_;
337  unsigned int ttc_transform_;
338  unsigned int ttc_transform_model_;
339  unsigned int ttc_extract_non_plane_;
340  unsigned int ttc_polygon_filter_;
341  unsigned int ttc_table_to_output_;
342  unsigned int ttc_cluster_objects_;
343  unsigned int ttc_visualization_;
344  unsigned int ttc_hungarian_;
345  unsigned int ttc_old_centroids_;
346  unsigned int ttc_obj_extraction_;
347 #endif
348 
349 #ifdef HAVE_VISUAL_DEBUGGING
351 #endif
352 };
353 
354 #endif
This class is used to save old centroids in order to check for reappearance.
OldCentroid & operator=(const OldCentroid &other)
Assignment operator.
Eigen::Vector4f centroid_
The position of centroid.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW unsigned int get_id() const
Get the ID of the centroid.
virtual ~OldCentroid()
Destructor.
OldCentroid(const unsigned int &id, const Eigen::Vector4f &centroid)
Constructor.
unsigned int id_
The ID of the centroid.
const Eigen::Vector4f & get_centroid() const
Get the position of the centroid.
unsigned int get_age() const
Get the age of the centroid.
OldCentroid(const OldCentroid &other)
Copy constructor.
unsigned int age_
The number of loops the centroid has been invisible for.
void increment_age()
Increment the age of the centroid.
Main thread of tabletop objects plugin.
virtual void finalize()
Finalize the thread.
virtual ~TabletopObjectsThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void init()
Initialize the thread.
Base class for virtualization thread.
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:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
Position3DInterface Fawkes BlackBoard Interface.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.