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
Thread aspect to access to BlackBoard.
Definition: blackboard.h:33
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:33
virtual void run()
Stub to see name in backtrace for easier debugging.
const Eigen::Vector4f & get_centroid() const
Get the position of the centroid.
unsigned int age_
The number of loops the centroid has been invisible for.
Main thread of tabletop objects plugin.
virtual ~OldCentroid()
Destructor.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:37
OldCentroid & operator=(const OldCentroid &other)
Assignment operator.
A class for handling time.
Definition: time.h:92
Eigen::Vector4f centroid_
The position of centroid.
Thread class encapsulation of pthreads.
Definition: thread.h:45
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:38
SwitchInterface Fawkes BlackBoard Interface.
Position3DInterface Fawkes BlackBoard Interface.
virtual void finalize()
Finalize the thread.
Time tracking utility.
Definition: tracker.h:36
Thread aspect to log output.
Definition: logging.h:32
unsigned int get_age() const
Get the age of the centroid.
OldCentroid(const unsigned int &id, const Eigen::Vector4f &centroid)
Constructor.
Thread aspect to access configuration data.
Definition: configurable.h:32
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.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW unsigned int get_id() const
Get the ID of the centroid.
void increment_age()
Increment the age of the centroid.