22 #ifndef _PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
23 #define _PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
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>
45 #include <Eigen/StdVector>
50 class Position3DInterface;
51 class SwitchInterface;
53 #ifdef USE_TIMETRACKER
58 #ifdef HAVE_VISUAL_DEBUGGING
73 OldCentroid(
const unsigned int &
id,
const Eigen::Vector4f ¢roid)
103 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
115 const Eigen::Vector4f &
162 #ifdef HAVE_VISUAL_DEBUGGING
167 typedef pcl::PointXYZ PointType;
170 typedef pcl::PointXYZRGB ColorPointType;
172 typedef Cloud::Ptr CloudPtr;
173 typedef Cloud::ConstPtr CloudConstPtr;
175 typedef ColorCloud::Ptr ColorCloudPtr;
176 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
178 typedef std::map<
unsigned int,
180 std::less<unsigned int>,
181 Eigen::aligned_allocator<std::pair<const unsigned int, Eigen::Vector4f>>>
183 typedef std::list<OldCentroid, Eigen::aligned_allocator<OldCentroid>> OldCentroidVector;
184 typedef std::vector<fawkes::Position3DInterface *> PosIfsVector;
189 const Eigen::Vector4f & centroid = Eigen::Vector4f(0, 0, 0, 0),
190 const Eigen::Quaternionf & rotation = Eigen::Quaternionf(1, 0, 0, 0));
192 CloudPtr simplify_polygon(CloudPtr polygon,
float sqr_dist_threshold);
193 CloudPtr generate_table_model(
const float length,
195 const float thickness,
197 const float max_error);
198 CloudPtr generate_table_model(
const float length,
201 const float max_error = 0.01);
202 bool is_polygon_edge_better(PointType &cb_br_p1p,
203 PointType &cb_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);
211 void convert_colored_input();
213 std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input);
215 ColorCloudPtr colorize_cluster(CloudConstPtr input_cloud,
216 const std::vector<int> &cluster,
217 const uint8_t color[]);
219 unsigned int cluster_objects(CloudConstPtr input,
220 ColorCloudPtr tmp_clusters,
221 std::vector<ColorCloudPtr> &tmp_obj_clusters);
224 void delete_old_centroids(OldCentroidVector centroids,
unsigned int age);
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 ¢roid,
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);
246 ColorCloudConstPtr colored_input_;
247 CloudPtr converted_input_;
248 CloudConstPtr input_;
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_;
256 std::map<unsigned int, signed int> best_obj_guess_;
259 double table_inclination_;
261 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> known_obj_dimensions_;
263 pcl::VoxelGrid<PointType> grid_;
264 pcl::SACSegmentation<PointType> seg_;
266 PosIfsVector pos_ifs_;
269 Eigen::Vector4f table_centroid;
271 std::list<unsigned int> free_ids_;
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_;
310 CloudPtr table_model_;
312 CloudPtr simplified_polygon_;
314 unsigned int loop_count_;
316 CentroidMap centroids_;
317 CentroidMap cylinder_params_;
318 OldCentroidVector old_centroids_;
321 std::map<uint, std::vector<double>> obj_likelihoods_;
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_;
349 #ifdef HAVE_VISUAL_DEBUGGING