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
49namespace fawkes {
50class Position3DInterface;
51class SwitchInterface;
52class Time;
53#ifdef USE_TIMETRACKER
54class 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{
67public:
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 */
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 */
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 &
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
137protected:
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,
153{
154public:
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
166private:
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
186private:
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() */
235protected:
236 virtual void
238 {
239 Thread::run();
240 }
241
242private:
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
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.
const Eigen::Vector4f & get_centroid() const
Get the position of the 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.
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.