Fawkes API Fawkes Development Version
tabletop_objects_thread.cpp
1
2/***************************************************************************
3 * tabletop_objects_thread.cpp - Thread to detect tabletop objects
4 *
5 * Created: Sat Nov 05 00:22:41 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#include "tabletop_objects_thread.h"
23
24#include "cluster_colors.h"
25#ifdef HAVE_VISUAL_DEBUGGING
26# include "visualization_thread_base.h"
27#endif
28
29#include <pcl_utils/comparisons.h>
30#include <pcl_utils/utils.h>
31#include <utils/math/angle.h>
32#include <utils/time/wait.h>
33#ifdef USE_TIMETRACKER
34# include <utils/time/tracker.h>
35#endif
36#include <interfaces/Position3DInterface.h>
37#include <interfaces/SwitchInterface.h>
38#include <pcl/ModelCoefficients.h>
39#include <pcl/common/centroid.h>
40#include <pcl/common/common.h>
41#include <pcl/common/distances.h>
42#include <pcl/common/transforms.h>
43#include <pcl/features/normal_3d.h>
44#include <pcl/filters/conditional_removal.h>
45#include <pcl/filters/extract_indices.h>
46#include <pcl/filters/passthrough.h>
47#include <pcl/filters/project_inliers.h>
48#include <pcl/filters/statistical_outlier_removal.h>
49#include <pcl/kdtree/kdtree.h>
50#include <pcl/kdtree/kdtree_flann.h>
51#include <pcl/point_cloud.h>
52#include <pcl/point_types.h>
53#include <pcl/registration/distances.h>
54#include <pcl/sample_consensus/method_types.h>
55#include <pcl/sample_consensus/model_types.h>
56#include <pcl/segmentation/extract_clusters.h>
57#include <pcl/surface/convex_hull.h>
58#include <utils/hungarian_method/hungarian.h>
59#include <utils/time/tracker_macros.h>
60
61#include <algorithm>
62#include <iostream>
63using namespace std;
64
65#define CFG_PREFIX "/perception/tabletop-objects/"
66
67/** @class TabletopObjectsThread "tabletop_objects_thread.h"
68 * Main thread of tabletop objects plugin.
69 * @author Tim Niemueller
70 */
71
72using namespace fawkes;
73
74/** Constructor. */
76: Thread("TabletopObjectsThread", Thread::OPMODE_CONTINUOUS),
77 TransformAspect(TransformAspect::ONLY_LISTENER)
78{
79#ifdef HAVE_VISUAL_DEBUGGING
80 visthread_ = NULL;
81#endif
82}
83
84/** Destructor. */
86{
87}
88
89void
91{
92 cfg_depth_filter_min_x_ = config->get_float(CFG_PREFIX "depth_filter_min_x");
93 cfg_depth_filter_max_x_ = config->get_float(CFG_PREFIX "depth_filter_max_x");
94 cfg_voxel_leaf_size_ = config->get_float(CFG_PREFIX "voxel_leaf_size");
95 cfg_segm_max_iterations_ = config->get_uint(CFG_PREFIX "table_segmentation_max_iterations");
96 cfg_segm_distance_threshold_ =
97 config->get_float(CFG_PREFIX "table_segmentation_distance_threshold");
98 cfg_segm_inlier_quota_ = config->get_float(CFG_PREFIX "table_segmentation_inlier_quota");
99 cfg_table_min_cluster_quota_ = config->get_float(CFG_PREFIX "table_min_cluster_quota");
100 cfg_table_downsample_leaf_size_ = config->get_float(CFG_PREFIX "table_downsample_leaf_size");
101 cfg_table_cluster_tolerance_ = config->get_float(CFG_PREFIX "table_cluster_tolerance");
102 cfg_max_z_angle_deviation_ = config->get_float(CFG_PREFIX "max_z_angle_deviation");
103 cfg_table_min_height_ = config->get_float(CFG_PREFIX "table_min_height");
104 cfg_table_max_height_ = config->get_float(CFG_PREFIX "table_max_height");
105 cfg_table_model_enable_ = config->get_bool(CFG_PREFIX "table_model_enable");
106 cfg_table_model_length_ = config->get_float(CFG_PREFIX "table_model_length");
107 cfg_table_model_width_ = config->get_float(CFG_PREFIX "table_model_width");
108 cfg_table_model_step_ = config->get_float(CFG_PREFIX "table_model_step");
109 cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX "horizontal_viewing_angle"));
110 cfg_vertical_va_ = deg2rad(config->get_float(CFG_PREFIX "vertical_viewing_angle"));
111 cfg_cluster_tolerance_ = config->get_float(CFG_PREFIX "cluster_tolerance");
112 cfg_cluster_min_size_ = config->get_uint(CFG_PREFIX "cluster_min_size");
113 cfg_cluster_max_size_ = config->get_uint(CFG_PREFIX "cluster_max_size");
114 cfg_base_frame_ = config->get_string("/frames/base");
115 cfg_result_frame_ = config->get_string(CFG_PREFIX "result_frame");
116 cfg_input_pointcloud_ = config->get_string(CFG_PREFIX "input_pointcloud");
117 cfg_centroid_max_age_ = config->get_uint(CFG_PREFIX "centroid_max_age");
118 cfg_centroid_max_distance_ = config->get_float(CFG_PREFIX "centroid_max_distance");
119 cfg_centroid_min_distance_ = config->get_float(CFG_PREFIX "centroid_min_distance");
120 cfg_centroid_max_height_ = config->get_float(CFG_PREFIX "centroid_max_height");
121 cfg_cylinder_fitting_ = config->get_bool(CFG_PREFIX "enable_cylinder_fitting");
122 cfg_track_objects_ = config->get_bool(CFG_PREFIX "enable_object_tracking");
123 try {
124 cfg_verbose_cylinder_fitting_ = config->get_bool(CFG_PREFIX "verbose_cylinder_fitting");
125 } catch (const Exception &e) {
126 cfg_verbose_cylinder_fitting_ = false;
127 }
128
129 if (pcl_manager->exists_pointcloud<PointType>(cfg_input_pointcloud_.c_str())) {
130 finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pointcloud_.c_str());
131 input_ = pcl_utils::cloudptr_from_refptr(finput_);
132 } else if (pcl_manager->exists_pointcloud<ColorPointType>(cfg_input_pointcloud_.c_str())) {
133 logger->log_warn(name(), "XYZ/RGB input point cloud, conversion required");
134 fcoloredinput_ = pcl_manager->get_pointcloud<ColorPointType>(cfg_input_pointcloud_.c_str());
135 colored_input_ = pcl_utils::cloudptr_from_refptr(fcoloredinput_);
136 converted_input_.reset(new Cloud());
137 input_ = converted_input_;
138 converted_input_->header.frame_id = colored_input_->header.frame_id;
139 converted_input_->header.stamp = colored_input_->header.stamp;
140 } else {
141 throw Exception("Point cloud '%s' does not exist or not XYZ or XYZ/RGB PCL",
142 cfg_input_pointcloud_.c_str());
143 }
144
145 try {
146 double rotation[4] = {0., 0., 0., 1.};
147 table_pos_if_ = NULL;
148 switch_if_ = NULL;
149
150 table_pos_if_ = blackboard->open_for_writing<Position3DInterface>("Tabletop");
151 table_pos_if_->set_rotation(rotation);
152 table_pos_if_->write();
153
154 switch_if_ = blackboard->open_for_writing<SwitchInterface>("tabletop-objects");
155 switch_if_->set_enabled(true);
156 switch_if_->write();
157
158 pos_ifs_.clear();
159 pos_ifs_.resize(MAX_CENTROIDS, NULL);
160 for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
161 char *tmp;
162 if (asprintf(&tmp, "Tabletop Object %u", i + 1) != -1) {
163 // Copy to get memory freed on exception
164 std::string id = tmp;
165 free(tmp);
167 pos_ifs_[i] = iface;
168 iface->set_rotation(rotation);
169 iface->write();
170 }
171 }
172 } catch (Exception &e) {
173 blackboard->close(table_pos_if_);
174 blackboard->close(switch_if_);
175 for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
176 if (pos_ifs_[i]) {
177 blackboard->close(pos_ifs_[i]);
178 }
179 }
180 throw;
181 }
182
183 fclusters_ = new pcl::PointCloud<ColorPointType>();
184 fclusters_->header.frame_id = input_->header.frame_id;
185 fclusters_->is_dense = false;
186 pcl_manager->add_pointcloud<ColorPointType>("tabletop-object-clusters", fclusters_);
187 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
188
189 char * tmp_name;
192 for (int i = 0; i < MAX_CENTROIDS; i++) {
193 f_tmp_cloud = new pcl::PointCloud<ColorPointType>();
194 f_tmp_cloud->header.frame_id = input_->header.frame_id;
195 f_tmp_cloud->is_dense = false;
196 std::string obj_id;
197 if (asprintf(&tmp_name, "obj_cluster_%u", i) != -1) {
198 obj_id = tmp_name;
199 free(tmp_name);
200 }
201 pcl_manager->add_pointcloud<ColorPointType>(obj_id.c_str(), f_tmp_cloud);
202 f_obj_clusters_.push_back(f_tmp_cloud);
203 tmp_cloud = pcl_utils::cloudptr_from_refptr(f_tmp_cloud);
204 obj_clusters_.push_back(tmp_cloud);
205 }
206
207 ////////////////////////////////////////////////////////////
208 //TODO must do initialization better (look-up table for known objects)
209 // obj_shape_confidence_.resize(MAX_CENTROIDS, 0.0);
210 NUM_KNOWN_OBJS_ = 3;
211 std::vector<double> init_likelihoods;
212 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
213 // TODO obj_likelihoods_ initialization
214 // obj_likelihoods_.resize(MAX_CENTROIDS, init_likelihoods);
215 // best_obj_guess_.resize(MAX_CENTROIDS, -1);
216
217 known_obj_dimensions_.resize(NUM_KNOWN_OBJS_);
218
219 //Green cup
220 known_obj_dimensions_[0][0] = 0.07;
221 known_obj_dimensions_[0][1] = 0.07;
222 known_obj_dimensions_[0][2] = 0.104;
223 //Red cup
224 known_obj_dimensions_[1][0] = 0.088;
225 known_obj_dimensions_[1][1] = 0.088;
226 known_obj_dimensions_[1][2] = 0.155;
227 //White cylinder
228 known_obj_dimensions_[2][0] = 0.106;
229 known_obj_dimensions_[2][1] = 0.106;
230 known_obj_dimensions_[2][2] = 0.277;
231
232 ////////////////////////////////////////////////////////////
233
234 table_inclination_ = 0.0;
235
236 ftable_model_ = new Cloud();
237 table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_);
238 table_model_->header.frame_id = input_->header.frame_id;
239 pcl_manager->add_pointcloud("tabletop-table-model", ftable_model_);
240 pcl_utils::set_time(ftable_model_, fawkes::Time(clock));
241
242 fsimplified_polygon_ = new Cloud();
243 simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_);
244 simplified_polygon_->header.frame_id = input_->header.frame_id;
245 pcl_manager->add_pointcloud("tabletop-simplified-polygon", fsimplified_polygon_);
246 pcl_utils::set_time(fsimplified_polygon_, fawkes::Time(clock));
247
248 grid_.setFilterFieldName("x");
249 grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_);
250 grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_);
251
252 seg_.setOptimizeCoefficients(true);
253 seg_.setModelType(pcl::SACMODEL_PLANE);
254 seg_.setMethodType(pcl::SAC_RANSAC);
255 seg_.setMaxIterations(cfg_segm_max_iterations_);
256 seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
257
258 loop_count_ = 0;
259
260 last_pcl_time_ = new Time(clock);
261
262 first_run_ = true;
263
264 old_centroids_.clear();
265 for (unsigned int i = 0; i < MAX_CENTROIDS; i++)
266 free_ids_.push_back(i);
267
268#ifdef USE_TIMETRACKER
269 tt_ = new TimeTracker();
270 tt_loopcount_ = 0;
271 ttc_full_loop_ = tt_->add_class("Full Loop");
272 ttc_msgproc_ = tt_->add_class("Message Processing");
273 ttc_convert_ = tt_->add_class("Input Conversion");
274 ttc_voxelize_ = tt_->add_class("Downsampling");
275 ttc_plane_ = tt_->add_class("Plane Segmentation");
276 ttc_extract_plane_ = tt_->add_class("Plane Extraction");
277 ttc_plane_downsampling_ = tt_->add_class("Plane Downsampling");
278 ttc_cluster_plane_ = tt_->add_class("Plane Clustering");
279 ttc_convex_hull_ = tt_->add_class("Convex Hull");
280 ttc_simplify_polygon_ = tt_->add_class("Polygon Simplification");
281 ttc_find_edge_ = tt_->add_class("Polygon Edge");
282 ttc_transform_ = tt_->add_class("Transform");
283 ttc_transform_model_ = tt_->add_class("Model Transformation");
284 ttc_extract_non_plane_ = tt_->add_class("Non-Plane Extraction");
285 ttc_polygon_filter_ = tt_->add_class("Polygon Filter");
286 ttc_table_to_output_ = tt_->add_class("Table to Output");
287 ttc_cluster_objects_ = tt_->add_class("Object Clustering");
288 ttc_visualization_ = tt_->add_class("Visualization");
289 ttc_hungarian_ = tt_->add_class("Hungarian Method (centroids)");
290 ttc_old_centroids_ = tt_->add_class("Old Centroid Removal");
291 ttc_obj_extraction_ = tt_->add_class("Object Extraction");
292#endif
293}
294
295void
297{
298 input_.reset();
299 clusters_.reset();
300 simplified_polygon_.reset();
301
302 pcl_manager->remove_pointcloud("tabletop-object-clusters");
303 pcl_manager->remove_pointcloud("tabletop-table-model");
304 pcl_manager->remove_pointcloud("tabletop-simplified-polygon");
305
306 blackboard->close(table_pos_if_);
307 blackboard->close(switch_if_);
308 for (PosIfsVector::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); ++it) {
309 blackboard->close(*it);
310 }
311 pos_ifs_.clear();
312
313 finput_.reset();
314 fclusters_.reset();
315 ftable_model_.reset();
316 fsimplified_polygon_.reset();
317
318 delete last_pcl_time_;
319#ifdef USE_TIMETRACKER
320 delete tt_;
321#endif
322}
323
324template <typename PointType>
325inline bool
326comparePoints2D(const PointType &p1, const PointType &p2)
327{
328 double angle1 = atan2(p1.y, p1.x) + M_PI;
329 double angle2 = atan2(p2.y, p2.x) + M_PI;
330 return (angle1 > angle2);
331}
332
333// Criteria for *not* choosing a segment:
334// 1. the existing current best is clearly closer in base-relative X direction
335// 2. the existing current best is longer
336bool
337TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p,
338 PointType &cb_br_p2p,
339 PointType &br_p1p,
340 PointType &br_p2p)
341{
342 // current best base-relative points
343 Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z);
344 Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z);
345 Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5;
346
347 Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z);
348 Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z);
349 Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5;
350
351 double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]);
352
353 // Criteria for *not* choosing a segment:
354 // 1. the existing current best is clearly closer in base-relative X direction
355 // 2. the existing current best is longer
356 if ((dist_x < -0.25)
357 || ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())))
358 return false;
359 else
360 return true;
361}
362
363void
365{
366 TIMETRACK_START(ttc_full_loop_);
367
368 ++loop_count_;
369
370 TIMETRACK_START(ttc_msgproc_);
371
372 while (!switch_if_->msgq_empty()) {
373 if (SwitchInterface::EnableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
374 switch_if_->set_enabled(true);
375 switch_if_->write();
376 } else if (SwitchInterface::DisableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
377 switch_if_->set_enabled(false);
378 switch_if_->write();
379 }
380
381 switch_if_->msgq_pop();
382 }
383
384 if (!switch_if_->is_enabled()) {
385 TimeWait::wait(250000);
386 TIMETRACK_ABORT(ttc_full_loop_);
387 return;
388 }
389
390 TIMETRACK_END(ttc_msgproc_);
391
392 fawkes::Time pcl_time;
393 if (colored_input_) {
394 pcl_utils::get_time(colored_input_, pcl_time);
395 } else {
396 pcl_utils::get_time(input_, pcl_time);
397 }
398 if (*last_pcl_time_ == pcl_time) {
399 TimeWait::wait(20000);
400 TIMETRACK_ABORT(ttc_full_loop_);
401 return;
402 }
403 *last_pcl_time_ = pcl_time;
404
405 if (colored_input_) {
406 TIMETRACK_START(ttc_convert_);
407 convert_colored_input();
408 TIMETRACK_END(ttc_convert_);
409 }
410
411 TIMETRACK_START(ttc_voxelize_);
412
413 CloudPtr temp_cloud(new Cloud);
414 CloudPtr temp_cloud2(new Cloud);
415 pcl::ExtractIndices<PointType> extract_;
416 CloudPtr cloud_hull_;
417 CloudPtr model_cloud_hull_;
418 CloudPtr cloud_proj_;
419 CloudPtr cloud_filt_;
420 CloudPtr cloud_above_;
421 CloudPtr cloud_objs_;
422 pcl::search::KdTree<PointType> kdtree_;
423
424 grid_.setInputCloud(input_);
425 grid_.filter(*temp_cloud);
426
427 if (temp_cloud->points.size() <= 10) {
428 // this can happen if run at startup. Since tabletop threads runs continuous
429 // and not synchronized with main loop, but point cloud acquisition thread is
430 // synchronized, we might start before any data has been read
431 //logger->log_warn(name(), "Empty voxelized point cloud, omitting loop");
432 TimeWait::wait(50000);
433 return;
434 }
435
436 TIMETRACK_INTER(ttc_voxelize_, ttc_plane_)
437
438 pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients());
439 pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
440 Eigen::Vector4f baserel_table_centroid(0, 0, 0, 0);
441
442 // This will search for the first plane which:
443 // 1. has a considerable amount of points (>= some percentage of input points)
444 // 2. is parallel to the floor (transformed normal angle to Z axis in specified epsilon)
445 // 3. is on a typical table height (at a specified height range in robot frame)
446 // Planes found along the way not satisfying any of the criteria are removed,
447 // the first plane either satisfying all criteria, or violating the first
448 // one end the loop
449 bool happy_with_plane = false;
450 while (!happy_with_plane) {
451 happy_with_plane = true;
452
453 if (temp_cloud->points.size() <= 10) {
455 "[L %u] no more points for plane detection, skipping loop",
456 loop_count_);
457 set_position(table_pos_if_, false);
458 TIMETRACK_ABORT(ttc_plane_);
459 TIMETRACK_ABORT(ttc_full_loop_);
460 TimeWait::wait(50000);
461 return;
462 }
463
464 seg_.setInputCloud(temp_cloud);
465 seg_.segment(*inliers, *coeff);
466
467 // 1. check for a minimum number of expected inliers
468 if ((double)inliers->indices.size()
469 < (cfg_segm_inlier_quota_ * (double)temp_cloud->points.size())) {
471 name(),
472 "[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)",
473 loop_count_,
474 inliers->indices.size(),
475 (cfg_segm_inlier_quota_ * temp_cloud->points.size()),
476 temp_cloud->points.size());
477 set_position(table_pos_if_, false);
478 TIMETRACK_ABORT(ttc_plane_);
479 TIMETRACK_ABORT(ttc_full_loop_);
480 TimeWait::wait(50000);
481 return;
482 }
483
484 // 2. Check angle between normal vector and Z axis of the
485 // base_link robot frame since tables are usually parallel to the ground...
486 try {
487 tf::Stamped<tf::Vector3> table_normal(tf::Vector3(coeff->values[0],
488 coeff->values[1],
489 coeff->values[2]),
490 fawkes::Time(0, 0),
491 input_->header.frame_id);
492
493 tf::Stamped<tf::Vector3> baserel_normal;
494 tf_listener->transform_vector(cfg_base_frame_, table_normal, baserel_normal);
495 tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z()));
496 table_inclination_ = z_axis.angle(baserel_normal);
497 if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_) {
498 happy_with_plane = false;
500 "[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding",
501 loop_count_,
502 baserel_normal.x(),
503 baserel_normal.y(),
504 baserel_normal.z(),
505 z_axis.angle(baserel_normal),
506 cfg_max_z_angle_deviation_);
507 }
508 } catch (Exception &e) {
509 logger->log_warn(name(), "Transforming normal failed, exception follows");
510 logger->log_warn(name(), e);
511 happy_with_plane = false;
512 }
513
514 if (happy_with_plane) {
515 // ok so far
516
517 // 3. Calculate table centroid, then transform it to the base_link system
518 // to make a table height sanity check, they tend to be at a specific height...
519 try {
520 pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid);
521 tf::Stamped<tf::Point> centroid(tf::Point(table_centroid[0],
522 table_centroid[1],
523 table_centroid[2]),
524 fawkes::Time(0, 0),
525 input_->header.frame_id);
526 tf::Stamped<tf::Point> baserel_centroid;
527 tf_listener->transform_point(cfg_base_frame_, centroid, baserel_centroid);
528 baserel_table_centroid[0] = baserel_centroid.x();
529 baserel_table_centroid[1] = baserel_centroid.y();
530 baserel_table_centroid[2] = baserel_centroid.z();
531
532 if ((baserel_centroid.z() < cfg_table_min_height_)
533 || (baserel_centroid.z() > cfg_table_max_height_)) {
534 happy_with_plane = false;
536 "[L %u] table height %f not in range [%f, %f]",
537 loop_count_,
538 baserel_centroid.z(),
539 cfg_table_min_height_,
540 cfg_table_max_height_);
541 }
542 } catch (tf::TransformException &e) {
543 //logger->log_warn(name(), "Transforming centroid failed, exception follows");
544 //logger->log_warn(name(), e);
545 }
546 }
547
548 if (!happy_with_plane) {
549 // throw away
550 Cloud extracted;
551 extract_.setNegative(true);
552 extract_.setInputCloud(temp_cloud);
553 extract_.setIndices(inliers);
554 extract_.filter(extracted);
555 *temp_cloud = extracted;
556 }
557 }
558
559 // If we got here we found the table
560 // Do NOT set it here, we will still try to determine the rotation as well
561 // set_position(table_pos_if_, true, table_centroid);
562
563 TIMETRACK_INTER(ttc_plane_, ttc_extract_plane_)
564
565 extract_.setNegative(false);
566 extract_.setInputCloud(temp_cloud);
567 extract_.setIndices(inliers);
568 extract_.filter(*temp_cloud2);
569
570 // Project the model inliers
571 pcl::ProjectInliers<PointType> proj;
572 proj.setModelType(pcl::SACMODEL_PLANE);
573 proj.setInputCloud(temp_cloud2);
574 proj.setModelCoefficients(coeff);
575 cloud_proj_.reset(new Cloud());
576 proj.filter(*cloud_proj_);
577
578 TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_);
579
580 // ***
581 // In the following cluster the projected table plane. This is done to get
582 // the largest continuous part of the plane to remove outliers, for instance
583 // if the intersection of the plane with a wall or object is taken into the
584 // table points.
585 // To achieve this cluster, if an acceptable cluster was found, extract this
586 // cluster as the new table points. Otherwise continue with the existing
587 // point cloud.
588
589 // further downsample table
590 CloudPtr cloud_table_voxelized(new Cloud());
591 pcl::VoxelGrid<PointType> table_grid;
592 table_grid.setLeafSize(cfg_table_downsample_leaf_size_,
593 cfg_table_downsample_leaf_size_,
594 cfg_table_downsample_leaf_size_);
595 table_grid.setInputCloud(cloud_proj_);
596 table_grid.filter(*cloud_table_voxelized);
597
598 TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_);
599
600 // Creating the KdTree object for the search method of the extraction
601 pcl::search::KdTree<PointType>::Ptr kdtree_table(new pcl::search::KdTree<PointType>());
602 kdtree_table->setInputCloud(cloud_table_voxelized);
603
604 std::vector<pcl::PointIndices> table_cluster_indices;
605 pcl::EuclideanClusterExtraction<PointType> table_ec;
606 table_ec.setClusterTolerance(cfg_table_cluster_tolerance_);
607 table_ec.setMinClusterSize(cfg_table_min_cluster_quota_ * cloud_table_voxelized->points.size());
608 table_ec.setMaxClusterSize(cloud_table_voxelized->points.size());
609 table_ec.setSearchMethod(kdtree_table);
610 table_ec.setInputCloud(cloud_table_voxelized);
611 table_ec.extract(table_cluster_indices);
612
613 if (!table_cluster_indices.empty()) {
614 // take the first, i.e. the largest cluster
615 CloudPtr cloud_table_extracted(new Cloud());
616 pcl::PointIndices::ConstPtr table_cluster_indices_ptr(
617 new pcl::PointIndices(table_cluster_indices[0]));
618 pcl::ExtractIndices<PointType> table_cluster_extract;
619 table_cluster_extract.setNegative(false);
620 table_cluster_extract.setInputCloud(cloud_table_voxelized);
621 table_cluster_extract.setIndices(table_cluster_indices_ptr);
622 table_cluster_extract.filter(*cloud_table_extracted);
623 *cloud_proj_ = *cloud_table_extracted;
624
625 // recompute based on the new chosen table cluster
626 pcl::compute3DCentroid(*cloud_proj_, table_centroid);
627
628 } else {
629 // Don't mess with the table, clustering didn't help to make it any better
631 "[L %u] table plane clustering did not generate any clusters",
632 loop_count_);
633 }
634
635 TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_)
636
637 // Estimate 3D convex hull -> TABLE BOUNDARIES
638 pcl::ConvexHull<PointType> hr;
639#ifdef PCL_VERSION_COMPARE
640# if PCL_VERSION_COMPARE(>=, 1, 5, 0)
641 hr.setDimension(2);
642# endif
643#endif
644
645 //hr.setAlpha(0.1); // only for ConcaveHull
646 hr.setInputCloud(cloud_proj_);
647 cloud_hull_.reset(new Cloud());
648 hr.reconstruct(*cloud_hull_);
649
650 if (cloud_hull_->points.empty()) {
651 logger->log_warn(name(), "[L %u] convex hull of table empty, skipping loop", loop_count_);
652 TIMETRACK_ABORT(ttc_convex_hull_);
653 TIMETRACK_ABORT(ttc_full_loop_);
654 set_position(table_pos_if_, false);
655 return;
656 }
657
658 TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_)
659
660 CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02);
661 *simplified_polygon_ = *simplified_polygon;
662 //logger->log_debug(name(), "Original polygon: %zu simplified: %zu", cloud_hull_->points.size(),
663 // simplified_polygon->points.size());
664 *cloud_hull_ = *simplified_polygon;
665
666 TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_)
667
668#ifdef HAVE_VISUAL_DEBUGGING
670 good_hull_edges.resize(cloud_hull_->points.size() * 2);
671#endif
672
673 try {
674 // Get transform Input camera -> base_link
676 fawkes::Time input_time(0, 0);
677 //pcl_utils::get_time(input_, input_time);
678 tf_listener->lookup_transform(cfg_base_frame_, input_->header.frame_id, input_time, t);
679
680 tf::Quaternion q = t.getRotation();
681 Eigen::Affine3f affine_cloud =
682 Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z())
683 * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z());
684
685 // Transform polygon cloud into base_link frame
686 CloudPtr baserel_polygon_cloud(new Cloud());
687 pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud);
688
689 // Setup plane normals for left, right, and lower frustrum
690 // planes for line segment verification
691 Eigen::Vector3f left_frustrum_normal =
692 Eigen::AngleAxisf(cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
693 * (-1. * Eigen::Vector3f::UnitY());
694
695 Eigen::Vector3f right_frustrum_normal =
696 Eigen::AngleAxisf(-cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
697 * Eigen::Vector3f::UnitY();
698
699 Eigen::Vector3f lower_frustrum_normal =
700 Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY())
701 * Eigen::Vector3f::UnitZ();
702
703 // point and good edge indexes of chosen candidate
704 size_t pidx1, pidx2;
705#ifdef HAVE_VISUAL_DEBUGGING
706 size_t geidx1 = std::numeric_limits<size_t>::max();
707 size_t geidx2 = std::numeric_limits<size_t>::max();
708#endif
709 // lower frustrum potential candidate
710 size_t lf_pidx1, lf_pidx2;
711 pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max();
712
713 // Search for good edges and backup edge candidates
714 // Good edges are the ones with either points close to
715 // two separate frustrum planes, and hence the actual line is
716 // well inside the frustrum, or with points inside the frustrum.
717 // Possible backup candidates are lines with both points
718 // close to the lower frustrum plane, i.e. lines cutoff by the
719 // vertical viewing angle. If we cannot determine a suitable edge
720 // otherwise we fallback to this line as it is a good rough guess
721 // to prevent at least worst things during manipulation
722 const size_t psize = cloud_hull_->points.size();
723#ifdef HAVE_VISUAL_DEBUGGING
724 size_t good_edge_points = 0;
725#endif
726 for (size_t i = 0; i < psize; ++i) {
727 //logger->log_debug(name(), "Checking %zu and %zu of %zu", i, (i+1) % psize, psize);
728 PointType &p1p = cloud_hull_->points[i];
729 PointType &p2p = cloud_hull_->points[(i + 1) % psize];
730
731 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
732 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
733
734 PointType &br_p1p = baserel_polygon_cloud->points[i];
735 PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize];
736
737 // check if both end points are close to left or right frustrum plane
738 if (!(((left_frustrum_normal.dot(p1) < 0.03) && (left_frustrum_normal.dot(p2) < 0.03))
739 || ((right_frustrum_normal.dot(p1) < 0.03)
740 && (right_frustrum_normal.dot(p2) < 0.03)))) {
741 // candidate edge, i.e. it's not too close to left or right frustrum planes
742
743 // check if both end points close to lower frustrum plane
744 if ((lower_frustrum_normal.dot(p1) < 0.01) && (lower_frustrum_normal.dot(p2) < 0.01)) {
745 // it's a lower frustrum line, keep just in case we do not
746 // find a better one
747 if ((lf_pidx1 == std::numeric_limits<size_t>::max())
748 || is_polygon_edge_better(br_p1p,
749 br_p2p,
750 baserel_polygon_cloud->points[lf_pidx1],
751 baserel_polygon_cloud->points[lf_pidx2])) {
752 // there was no backup candidate, yet, or this one is closer
753 // to the robot, take it.
754 lf_pidx1 = i;
755 lf_pidx2 = (i + 1) % psize;
756 }
757
758 continue;
759 }
760
761#ifdef HAVE_VISUAL_DEBUGGING
762 // Remember as good edge for visualization
763 for (unsigned int j = 0; j < 3; ++j)
764 good_hull_edges[good_edge_points][j] = p1[j];
765 good_hull_edges[good_edge_points][3] = 0.;
766 ++good_edge_points;
767 for (unsigned int j = 0; j < 3; ++j)
768 good_hull_edges[good_edge_points][j] = p2[j];
769 good_hull_edges[good_edge_points][3] = 0.;
770 ++good_edge_points;
771#endif
772
773 if (pidx1 != std::numeric_limits<size_t>::max()) {
774 // current best base-relative points
775 PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1];
776 PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2];
777
778 if (!is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p)) {
779 //logger->log_info(name(), "Skipping: cb(%f,%f)->(%f,%f) c(%f,%f)->(%f,%f)",
780 // cb_br_p1p.x, cb_br_p1p.y, cb_br_p2p.x, cb_br_p2p.y,
781 // br_p1p.x, br_p1p.y, br_p2p.x, br_p2p.y);
782 continue;
783 } else {
784 //logger->log_info(name(), "Taking: cb(%f,%f)->(%f,%f) c(%f,%f)->(%f,%f)",
785 // cb_br_p1p.x, cb_br_p1p.y, cb_br_p2p.x, cb_br_p2p.y,
786 // br_p1p.x, br_p1p.y, br_p2p.x, br_p2p.y);
787 }
788 //} else {
789 //logger->log_info(name(), "Taking because we had none");
790 }
791
792 // Was not sorted out, therefore promote candidate to current best
793 pidx1 = i;
794 pidx2 = (i + 1) % psize;
795#ifdef HAVE_VISUAL_DEBUGGING
796 geidx1 = good_edge_points - 2;
797 geidx2 = good_edge_points - 1;
798#endif
799 }
800 }
801
802 //logger->log_debug(name(), "Current best is %zu -> %zu", pidx1, pidx2);
803
804 // in the case we have a backup lower frustrum edge check if we should use it
805 // Criteria:
806 // 0. we have a backup point
807 // 1. no other suitable edge was chosen at all
808 // 2. angle(Y_axis, chosen_edge) > threshold
809 // 3.. p1.x or p2.x > centroid.x
810 if (lf_pidx1 != std::numeric_limits<size_t>::max()) {
811 PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1];
812 PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2];
813
814 Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.);
815 Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.);
816
817 // None found at all
818 if (pidx1 == std::numeric_limits<size_t>::max()) {
819 pidx1 = lf_pidx1;
820 pidx2 = lf_pidx2;
821
822#ifdef HAVE_VISUAL_DEBUGGING
823 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
824 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
825 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
826 geidx1 = good_edge_points++;
827
828 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
829 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
830 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
831 geidx2 = good_edge_points++;
832#endif
833
834 } else {
835 PointType &p1p = baserel_polygon_cloud->points[pidx1];
836 PointType &p2p = baserel_polygon_cloud->points[pidx2];
837
838 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.);
839 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.);
840
841 // Unsuitable "good" line until now?
842 if ( //(pcl::getAngle3D(p2 - p1, Eigen::Vector4f::UnitZ()) > M_PI * 0.5) ||
843 (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0])) {
844 //logger->log_warn(name(), "Choosing backup candidate!");
845
846 pidx1 = lf_pidx1;
847 pidx2 = lf_pidx2;
848
849#ifdef HAVE_VISUAL_DEBUGGING
850 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
851 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
852 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
853 geidx1 = good_edge_points++;
854
855 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
856 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
857 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
858 geidx2 = good_edge_points++;
859#endif
860 }
861 }
862 }
863
864 //logger->log_info(name(), "Chose %zu -> %zu", pidx1, pidx2);
865
866#ifdef HAVE_VISUAL_DEBUGGING
867 if (good_edge_points > 0) {
868 good_hull_edges[geidx1][3] = 1.0;
869 good_hull_edges[geidx2][3] = 1.0;
870 }
871 good_hull_edges.resize(good_edge_points);
872#endif
873
874 TIMETRACK_END(ttc_find_edge_);
875
876 model_cloud_hull_.reset(new Cloud());
877 if (cfg_table_model_enable_ && (pidx1 != std::numeric_limits<size_t>::max())
878 && (pidx2 != std::numeric_limits<size_t>::max())) {
879 TIMETRACK_START(ttc_transform_);
880
881 // Calculate transformation parameters based on determined
882 // convex hull polygon segment we decided on as "the table edge"
883 PointType &p1p = cloud_hull_->points[pidx1];
884 PointType &p2p = cloud_hull_->points[pidx2];
885
886 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
887 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
888
889 // Normal vectors for table model and plane
890 Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ();
891 Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]);
892 normal.normalize(); // just in case
893
894 Eigen::Vector3f table_centroid_3f =
895 Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]);
896
897 // Rotational parameters to align table to polygon segment
898 Eigen::Vector3f p1_p2 = p2 - p1;
899 Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5;
900 p1_p2.normalize();
901 Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal);
902 p1_p2_normal_cross.normalize();
903
904 // For N=(A,B,C), and hessian Ax+By+Cz+D=0 and N dot X=(Ax+By+Cz)
905 // we get N dot X + D = 0 -> -D = N dot X
906 double nD = -p1_p2_normal_cross.dot(p1_p2_center);
907 double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD;
908 if (p1_p2_centroid_dist < 0) {
909 // normal points to the "wrong" side fo our purpose
910 p1_p2_normal_cross *= -1;
911 }
912
913 Eigen::Vector3f table_center =
914 p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5);
915
916 for (unsigned int i = 0; i < 3; ++i)
917 table_centroid[i] = table_center[i];
918 table_centroid[3] = 0.;
919
920 // calculate table corner points
921 std::vector<Eigen::Vector3f> tpoints(4);
922 tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5);
923 tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_;
924 tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5);
925 tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_;
926
927 model_cloud_hull_->points.resize(4);
928 model_cloud_hull_->height = 1;
929 model_cloud_hull_->width = 4;
930 model_cloud_hull_->is_dense = true;
931 for (int i = 0; i < 4; ++i) {
932 model_cloud_hull_->points[i].x = tpoints[i][0];
933 model_cloud_hull_->points[i].y = tpoints[i][1];
934 model_cloud_hull_->points[i].z = tpoints[i][2];
935 }
936 //std::sort(model_cloud_hull_->points.begin(),
937 // model_cloud_hull_->points.end(), comparePoints2D<PointType>);
938
939 // Rotational parameters to rotate table model from camera to
940 // determined table position in 3D space
941 Eigen::Vector3f rotaxis = model_normal.cross(normal);
942 rotaxis.normalize();
943 double angle = acos(normal.dot(model_normal));
944
945 // Transformation to translate model from camera center into actual pose
946 Eigen::Affine3f affine =
947 Eigen::Translation3f(table_centroid.x(), table_centroid.y(), table_centroid.z())
948 * Eigen::AngleAxisf(angle, rotaxis);
949
950 Eigen::Vector3f model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.),
951 model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.);
952 model_p1 = affine * model_p1;
953 model_p2 = affine * model_p2;
954
955 // Calculate the vector between model_p1 and model_p2
956 Eigen::Vector3f model_p1_p2 = model_p2 - model_p1;
957 model_p1_p2.normalize();
958 // Calculate rotation axis between model_p1 and model_p2
959 Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2);
960 model_rotaxis.normalize();
961 double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2));
962 //logger->log_info(name(), "Angle: %f Poly (%f,%f,%f) -> (%f,%f,%f) model (%f,%f,%f) -> (%f,%f,%f)",
963 // angle_p1_p2, p1.x(), p1.y(), p1.z(), p2.x(), p2.y(), p2.z(),
964 // model_p1.x(), model_p1.y(), model_p1.z(), model_p2.x(), model_p2.y(), model_p2.z());
965
966 // Final full transformation of the table within the camera coordinate frame
967 affine = Eigen::Translation3f(table_centroid.x(), table_centroid.y(), table_centroid.z())
968 * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) * Eigen::AngleAxisf(angle, rotaxis);
969
970 // Just the rotational part
971 Eigen::Quaternionf qt;
972 qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) * Eigen::AngleAxisf(angle, rotaxis);
973
974 // Set position again, this time with the rotation
975 set_position(table_pos_if_, true, table_centroid, qt);
976
977 TIMETRACK_INTER(ttc_transform_, ttc_transform_model_)
978
979 // to show fitted table model
980 CloudPtr table_model = generate_table_model(cfg_table_model_length_,
981 cfg_table_model_width_,
982 cfg_table_model_step_);
983 pcl::transformPointCloud(*table_model, *table_model_, affine.matrix());
984 //*table_model_ = *model_cloud_hull_;
985 //*table_model_ = *table_model;
986 table_model_->header.frame_id = input_->header.frame_id;
987
988 TIMETRACK_END(ttc_transform_model_);
989 }
990
991 } catch (Exception &e) {
992 set_position(table_pos_if_, false);
993 logger->log_warn(name(), "Failed to transform convex hull cloud, exception follows");
994 logger->log_warn(name(), e);
995 TIMETRACK_ABORT(ttc_find_edge_);
996 }
997
998 TIMETRACK_START(ttc_extract_non_plane_);
999 // Extract all non-plane points
1000 cloud_filt_.reset(new Cloud());
1001 extract_.setNegative(true);
1002 extract_.filter(*cloud_filt_);
1003
1004 TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_);
1005
1006 // Check if the viewpoint, i.e. the input point clouds frame origin,
1007 // if above or below the table centroid. If it is above, we want to point
1008 // the normal towards the viewpoint in the next steps, otherwise it
1009 // should point away from the sensor. "Above" is relative to the base link
1010 // frame, i.e. the frame that is based on the ground support plane with the
1011 // Z axis pointing upwards
1012 bool viewpoint_above = true;
1013 try {
1014 tf::Stamped<tf::Point> origin(tf::Point(0, 0, 0), fawkes::Time(0, 0), input_->header.frame_id);
1015 tf::Stamped<tf::Point> baserel_viewpoint;
1016 tf_listener->transform_point(cfg_base_frame_, origin, baserel_viewpoint);
1017
1018 viewpoint_above = (baserel_viewpoint.z() > table_centroid[2]);
1019 } catch (tf::TransformException &e) {
1020 logger->log_warn(name(), "[L %u] could not transform viewpoint to base link", loop_count_);
1021 }
1022
1023 // Use only points above tables
1024 // Why coeff->values[3] > 0 ? ComparisonOps::GT : ComparisonOps::LT?
1025 // The model coefficients are in Hessian Normal Form, hence coeff[0..2] are
1026 // the normal vector. We need to distinguish the cases where the normal vector
1027 // points towards the origin (camera) or away from it. This can be checked
1028 // by calculating the distance towards the origin, which conveniently in
1029 // dist = N * x + p is just p which is coeff[3]. Therefore, if coeff[3] is
1030 // positive, the normal vector points towards the camera and we want all
1031 // points with positive distance from the table plane, otherwise it points
1032 // away from the origin and we want points with "negative distance".
1033 // We make use of the fact that we only have a boring RGB-D camera and
1034 // not an X-Ray...
1035 pcl::ComparisonOps::CompareOp op =
1036 viewpoint_above ? (coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT)
1037 : (coeff->values[3] < 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT);
1040 pcl::ConditionAnd<PointType>::Ptr above_cond(new pcl::ConditionAnd<PointType>());
1041 above_cond->addComparison(above_comp);
1042 pcl::ConditionalRemoval<PointType> above_condrem;
1043 above_condrem.setCondition(above_cond);
1044 above_condrem.setInputCloud(cloud_filt_);
1045 cloud_above_.reset(new Cloud());
1046 above_condrem.filter(*cloud_above_);
1047
1048 //printf("Before: %zu After: %zu\n", cloud_filt_->points.size(),
1049 // cloud_above_->points.size());
1050 if (cloud_filt_->points.size() < cfg_cluster_min_size_) {
1051 //logger->log_warn(name(), "Less points than cluster min size");
1052 TIMETRACK_ABORT(ttc_polygon_filter_);
1053 TIMETRACK_ABORT(ttc_full_loop_);
1054 return;
1055 }
1056
1057 // Extract only points on the table plane
1058 pcl::PointIndices::Ptr polygon(new pcl::PointIndices());
1059
1060 pcl::ConditionAnd<PointType>::Ptr polygon_cond(new pcl::ConditionAnd<PointType>());
1061
1064 (model_cloud_hull_ && !model_cloud_hull_->points.empty()) ? *model_cloud_hull_
1065 : *cloud_hull_));
1066 polygon_cond->addComparison(inpoly_comp);
1067
1068 // build the filter
1069 pcl::ConditionalRemoval<PointType> condrem;
1070 condrem.setCondition(polygon_cond);
1071 condrem.setInputCloud(cloud_above_);
1072 //condrem.setKeepOrganized(true);
1073 cloud_objs_.reset(new Cloud());
1074 condrem.filter(*cloud_objs_);
1075
1076 //CloudPtr table_points(new Cloud());
1077 //condrem.setInputCloud(temp_cloud2);
1078 //condrem.filter(*table_points);
1079
1080 // CLUSTERS
1081 // extract clusters of OBJECTS
1082
1083 TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_);
1084
1085 std::vector<int> indices(cloud_proj_->points.size());
1086 for (uint i = 0; i < indices.size(); i++)
1087 indices[i] = i;
1088 ColorCloudPtr tmp_clusters = colorize_cluster(cloud_proj_, indices, table_color);
1089 tmp_clusters->height = 1;
1090 tmp_clusters->is_dense = false;
1091 tmp_clusters->width = cloud_proj_->points.size();
1092
1093 TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_);
1094
1095 unsigned int object_count = 0;
1096 if (cloud_objs_->points.size() > 0) {
1097 //TODO: perform statistical outlier removal at this point before clustering.
1098 //Outlier removal
1099 pcl::StatisticalOutlierRemoval<PointType> sor;
1100 sor.setInputCloud(cloud_objs_);
1101 sor.setMeanK(5);
1102 sor.setStddevMulThresh(0.2);
1103 sor.filter(*cloud_objs_);
1104 }
1105 //OBJECTS
1106 std::vector<pcl::PointCloud<ColorPointType>::Ptr> tmp_obj_clusters(MAX_CENTROIDS);
1107 object_count = cluster_objects(cloud_objs_, tmp_clusters, tmp_obj_clusters);
1108 if (object_count == 0) {
1109 logger->log_info(name(), "No clustered points found");
1110 }
1111
1112 TIMETRACK_INTER(ttc_hungarian_, ttc_old_centroids_)
1113
1114 // age all old centroids
1115 for (OldCentroidVector::iterator it = old_centroids_.begin(); it != old_centroids_.end(); ++it) {
1116 it->increment_age();
1117 }
1118 // delete centroids which are older than cfg_centroid_max_age_
1119 delete_old_centroids(old_centroids_, cfg_centroid_max_age_);
1120 // delete old centroids which are too close to current centroids
1121 delete_near_centroids(centroids_, old_centroids_, cfg_centroid_min_distance_);
1122
1123 TIMETRACK_END(ttc_old_centroids_);
1124
1125 // set all pos_ifs not in centroids_ to 'not visible'
1126 for (unsigned int i = 0; i < pos_ifs_.size(); i++) {
1127 if (!centroids_.count(i)) {
1128 set_position(pos_ifs_[i], false);
1129 }
1130 }
1131
1132 // set positions of all visible centroids
1133 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
1134 set_position(pos_ifs_[it->first], true, it->second);
1135 }
1136
1137 TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_)
1138
1139 *clusters_ = *tmp_clusters;
1140 fclusters_->header.frame_id = input_->header.frame_id;
1141 pcl_utils::copy_time(input_, fclusters_);
1142 pcl_utils::copy_time(input_, ftable_model_);
1143 pcl_utils::copy_time(input_, fsimplified_polygon_);
1144
1145 for (unsigned int i = 0; i < f_obj_clusters_.size(); i++) {
1146 if (centroids_.count(i)) {
1147 *obj_clusters_[i] = *tmp_obj_clusters[i];
1148 } else {
1149 obj_clusters_[i]->clear();
1150 // add point to force update
1151 // TODO find proper way to update an empty cloud
1152 // obj_clusters_[i]->push_back(ColorPointType());
1153 }
1154 pcl_utils::copy_time(input_, f_obj_clusters_[i]);
1155 }
1156
1157#ifdef HAVE_VISUAL_DEBUGGING
1158 if (visthread_) {
1159 Eigen::Vector4f normal;
1160 normal[0] = coeff->values[0];
1161 normal[1] = coeff->values[1];
1162 normal[2] = coeff->values[2];
1163 normal[3] = 0.;
1164
1166 hull_vertices.resize(cloud_hull_->points.size());
1167 for (unsigned int i = 0; i < cloud_hull_->points.size(); ++i) {
1168 hull_vertices[i][0] = cloud_hull_->points[i].x;
1169 hull_vertices[i][1] = cloud_hull_->points[i].y;
1170 hull_vertices[i][2] = cloud_hull_->points[i].z;
1171 hull_vertices[i][3] = 0.;
1172 }
1173
1175 if (model_cloud_hull_ && !model_cloud_hull_->points.empty()) {
1176 model_vertices.resize(model_cloud_hull_->points.size());
1177 for (unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) {
1178 model_vertices[i][0] = model_cloud_hull_->points[i].x;
1179 model_vertices[i][1] = model_cloud_hull_->points[i].y;
1180 model_vertices[i][2] = model_cloud_hull_->points[i].z;
1181 model_vertices[i][3] = 0.;
1182 }
1183 }
1184
1185 visthread_->visualize(input_->header.frame_id,
1186 table_centroid,
1187 normal,
1188 hull_vertices,
1189 model_vertices,
1190 good_hull_edges,
1191 centroids_,
1192 cylinder_params_,
1193 obj_shape_confidence_,
1194 best_obj_guess_);
1195 }
1196#endif
1197
1198 TIMETRACK_END(ttc_visualization_);
1199 TIMETRACK_END(ttc_full_loop_);
1200
1201#ifdef USE_TIMETRACKER
1202 if (++tt_loopcount_ >= 5) {
1203 tt_loopcount_ = 0;
1204 tt_->print_to_stdout();
1205 }
1206#endif
1207}
1208
1209std::vector<pcl::PointIndices>
1210TabletopObjectsThread::extract_object_clusters(CloudConstPtr input)
1211{
1212 TIMETRACK_START(ttc_obj_extraction_);
1213 std::vector<pcl::PointIndices> cluster_indices;
1214 if (input->empty()) {
1215 TIMETRACK_ABORT(ttc_obj_extraction_);
1216 return cluster_indices;
1217 }
1218 // Creating the KdTree object for the search method of the extraction
1219
1220 pcl::search::KdTree<PointType>::Ptr kdtree_cl(new pcl::search::KdTree<PointType>());
1221 kdtree_cl->setInputCloud(input);
1222
1223 pcl::EuclideanClusterExtraction<PointType> ec;
1224 ec.setClusterTolerance(cfg_cluster_tolerance_);
1225 ec.setMinClusterSize(cfg_cluster_min_size_);
1226 ec.setMaxClusterSize(cfg_cluster_max_size_);
1227 ec.setSearchMethod(kdtree_cl);
1228 ec.setInputCloud(input);
1229 ec.extract(cluster_indices);
1230
1231 //logger->log_debug(name(), "Found %zu clusters", cluster_indices.size());
1232 TIMETRACK_END(ttc_obj_extraction_);
1233 return cluster_indices;
1234}
1235
1236int
1237TabletopObjectsThread::next_id()
1238{
1239 if (free_ids_.empty()) {
1240 logger->log_debug(name(), "free_ids is empty");
1241 return -1;
1242 }
1243 int id = free_ids_.front();
1244 free_ids_.pop_front();
1245 return id;
1246}
1247
1248unsigned int
1249TabletopObjectsThread::cluster_objects(CloudConstPtr input_cloud,
1250 ColorCloudPtr tmp_clusters,
1251 std::vector<ColorCloudPtr> &tmp_obj_clusters)
1252{
1253 unsigned int object_count = 0;
1254 std::vector<pcl::PointIndices> cluster_indices = extract_object_clusters(input_cloud);
1255 std::vector<pcl::PointIndices>::const_iterator it;
1256 unsigned int num_points = 0;
1257 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
1258 num_points += it->indices.size();
1259
1260 CentroidMap tmp_centroids;
1261
1262 if (num_points > 0) {
1263 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids(
1264 MAX_CENTROIDS);
1265
1266 unsigned int centroid_i = 0;
1267
1268 std::vector<double> init_likelihoods;
1269 init_likelihoods.resize(NUM_KNOWN_OBJS_ + 1, 0.0);
1270 for (uint i = 0; i < MAX_CENTROIDS; i++)
1271 obj_likelihoods_[i] = init_likelihoods;
1272
1273 for (it = cluster_indices.begin(); it != cluster_indices.end() && centroid_i < MAX_CENTROIDS;
1274 ++it, ++centroid_i) {
1276 "********************Processing obj_%u********************",
1277 centroid_i);
1278
1279 //Centroids in cam frame:
1280 //pcl::compute3DCentroid(*cloud_objs_, it->indices, centroids[centroid_i]);
1281
1282 // TODO fix this; we only want to copy the cluster, the color is incorrect
1283 ColorCloudPtr single_cluster =
1284 colorize_cluster(input_cloud, it->indices, cluster_colors[centroid_i]);
1285 single_cluster->header.frame_id = input_cloud->header.frame_id;
1286 single_cluster->width = it->indices.size();
1287 single_cluster->height = 1;
1288
1289 ColorCloudPtr obj_in_base_frame(new ColorCloud());
1290 obj_in_base_frame->header.frame_id = cfg_base_frame_;
1291 obj_in_base_frame->width = it->indices.size();
1292 obj_in_base_frame->height = 1;
1293 obj_in_base_frame->points.resize(it->indices.size());
1294
1295 // don't add cluster here since the id is wrong
1296 //*obj_clusters_[obj_i++] = *single_cluster;
1297
1298 pcl_utils::transform_pointcloud(cfg_base_frame_,
1299 *single_cluster,
1300 *obj_in_base_frame,
1301 *tf_listener);
1302
1303 pcl::compute3DCentroid(*obj_in_base_frame, new_centroids[centroid_i]);
1304
1305 if (cfg_cylinder_fitting_) {
1306 new_centroids[centroid_i] =
1307 fit_cylinder(obj_in_base_frame, new_centroids[centroid_i], centroid_i);
1308 }
1309 }
1310 object_count = centroid_i;
1311 new_centroids.resize(object_count);
1312
1313 // save cylinder fitting variables
1314 // to temporary variables to be able to reassign IDs
1315 CentroidMap cylinder_params(cylinder_params_);
1316 std::map<uint, signed int> best_obj_guess(best_obj_guess_);
1317 std::map<uint, double> obj_shape_confidence(obj_shape_confidence_);
1318 std::map<uint, std::vector<double>> obj_likelihoods(obj_likelihoods_);
1319 cylinder_params_.clear();
1320 best_obj_guess_.clear();
1321 obj_shape_confidence_.clear();
1322 obj_likelihoods_.clear();
1323
1324 std::map<uint, int> assigned_ids;
1325 if (cfg_track_objects_) {
1326 assigned_ids = track_objects(new_centroids);
1327 } else { //! cfg_track_objects_
1328 for (unsigned int i = 0; i < new_centroids.size(); i++) {
1329 assigned_ids[i] = i;
1330 }
1331 }
1332
1333 // reassign IDs
1334 for (uint i = 0; i < new_centroids.size(); i++) {
1335 int assigned_id;
1336 try {
1337 assigned_id = assigned_ids.at(i);
1338 } catch (const std::out_of_range &e) {
1339 logger->log_error(name(), "Object %d was not assigned", i);
1340 // drop centroid
1341 assigned_id = -1;
1342 }
1343 if (assigned_id == -1)
1344 continue;
1345 tmp_centroids[assigned_id] = new_centroids[i];
1346 cylinder_params_[assigned_id] = cylinder_params[i];
1347 obj_shape_confidence_[assigned_id] = obj_shape_confidence[i];
1348 best_obj_guess_[assigned_id] = best_obj_guess[i];
1349 obj_likelihoods_[assigned_id] = obj_likelihoods[i];
1350 ColorCloudPtr colorized_cluster =
1351 colorize_cluster(input_cloud,
1352 cluster_indices[i].indices,
1353 cluster_colors[assigned_id % MAX_CENTROIDS]);
1354 *tmp_clusters += *colorized_cluster;
1355 tmp_obj_clusters[assigned_id] = colorized_cluster;
1356 }
1357
1358 // remove all centroids too high above the table
1359 remove_high_centroids(table_centroid, tmp_centroids);
1360
1361 if (object_count > 0)
1362 first_run_ = false;
1363 } else {
1364 logger->log_info(name(), "No clustered points found");
1365 // save all centroids to old centroids
1366 for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); ++it) {
1367 old_centroids_.push_back(OldCentroid(it->first, it->second));
1368 }
1369 }
1370 centroids_ = tmp_centroids;
1371 return object_count;
1372}
1373
1374TabletopObjectsThread::ColorCloudPtr
1375TabletopObjectsThread::colorize_cluster(CloudConstPtr input_cloud,
1376 const std::vector<int> &cluster,
1377 const uint8_t color[])
1378{
1379 ColorCloudPtr result(new ColorCloud());
1380 result->resize(cluster.size());
1381 result->header.frame_id = input_cloud->header.frame_id;
1382 uint i = 0;
1383 for (std::vector<int>::const_iterator it = cluster.begin(); it != cluster.end(); ++it, ++i) {
1384 ColorPointType & p1 = result->points.at(i);
1385 const PointType &p2 = input_cloud->points.at(*it);
1386 p1.x = p2.x;
1387 p1.y = p2.y;
1388 p1.z = p2.z;
1389 p1.r = color[0];
1390 p1.g = color[1];
1391 p1.b = color[2];
1392 }
1393 return result;
1394}
1395
1396bool
1397TabletopObjectsThread::compute_bounding_box_scores(
1398 Eigen::Vector3f & cluster_dim,
1399 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> &scores)
1400{
1401 scores.resize(NUM_KNOWN_OBJS_);
1402
1403 for (int i = 0; i < NUM_KNOWN_OBJS_; i++) {
1404 scores[i][0] = compute_similarity(cluster_dim[0], known_obj_dimensions_[i][0]);
1405 scores[i][1] = compute_similarity(cluster_dim[1], known_obj_dimensions_[i][1]);
1406 scores[i][2] = compute_similarity(cluster_dim[2], known_obj_dimensions_[i][2]);
1407 }
1408 return true;
1409}
1410
1411double
1412TabletopObjectsThread::compute_similarity(double d1, double d2)
1413{
1414 return exp(-50.0 * ((d1 - d2) * (d1 - d2)));
1415}
1416
1417void
1418TabletopObjectsThread::set_position(fawkes::Position3DInterface *iface,
1419 bool is_visible,
1420 const Eigen::Vector4f & centroid,
1421 const Eigen::Quaternionf & attitude)
1422{
1423 tf::Stamped<tf::Pose> baserel_pose;
1424 try {
1426 tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
1427 tf::Vector3(centroid[0], centroid[1], centroid[2])),
1428 fawkes::Time(0, 0),
1429 input_->header.frame_id);
1430 tf_listener->transform_pose(cfg_result_frame_, spose, baserel_pose);
1431 iface->set_frame(cfg_result_frame_.c_str());
1432 } catch (Exception &e) {
1433 is_visible = false;
1434 }
1435
1436 int visibility_history = iface->visibility_history();
1437 if (is_visible) {
1438 if (visibility_history >= 0) {
1439 iface->set_visibility_history(visibility_history + 1);
1440 } else {
1441 iface->set_visibility_history(1);
1442 }
1443 tf::Vector3 & origin = baserel_pose.getOrigin();
1444 tf::Quaternion quat = baserel_pose.getRotation();
1445 double translation[3] = {origin.x(), origin.y(), origin.z()};
1446 double rotation[4] = {quat.x(), quat.y(), quat.z(), quat.w()};
1447 iface->set_translation(translation);
1448 iface->set_rotation(rotation);
1449
1450 } else {
1451 if (visibility_history <= 0) {
1452 iface->set_visibility_history(visibility_history - 1);
1453 } else {
1454 iface->set_visibility_history(-1);
1455 double translation[3] = {0, 0, 0};
1456 double rotation[4] = {0, 0, 0, 1};
1457 iface->set_translation(translation);
1458 iface->set_rotation(rotation);
1459 }
1460 }
1461 iface->write();
1462}
1463
1464TabletopObjectsThread::CloudPtr
1465TabletopObjectsThread::generate_table_model(const float length,
1466 const float width,
1467 const float thickness,
1468 const float step,
1469 const float max_error)
1470{
1471 CloudPtr c(new Cloud());
1472
1473 const float length_2 = fabs(length) * 0.5;
1474 const float width_2 = fabs(width) * 0.5;
1475 const float thickness_2 = fabs(thickness) * 0.5;
1476
1477 // calculate table points
1478 const unsigned int l_base_num = std::max(2u, (unsigned int)floor(length / step));
1479 const unsigned int num_w =
1480 l_base_num
1481 + ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1482 const unsigned int w_base_num = std::max(2u, (unsigned int)floor(width / step));
1483 const unsigned int num_h =
1484 w_base_num
1485 + ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1486 const unsigned int t_base_num = std::max(2u, (unsigned int)floor(thickness / step));
1487 const unsigned int num_t =
1488 t_base_num
1489 + ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1));
1490
1491 //logger->log_debug(name(), "Generating table model %fx%fx%f (%ux%ux%u=%u points)",
1492 // length, width, thickness,
1493 // num_w, num_h, num_t, num_t * num_w * num_h);
1494
1495 c->height = 1;
1496 c->width = num_t * num_w * num_h;
1497 c->is_dense = true;
1498 c->points.resize((size_t)num_t * (size_t)num_w * (size_t)num_h);
1499
1500 unsigned int idx = 0;
1501 for (unsigned int t = 0; t < num_t; ++t) {
1502 for (unsigned int w = 0; w < num_w; ++w) {
1503 for (unsigned int h = 0; h < num_h; ++h) {
1504 PointType &p = c->points[idx++];
1505
1506 p.x = h * step - width_2;
1507 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error)
1508 p.x = width_2;
1509
1510 p.y = w * step - length_2;
1511 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error)
1512 p.y = length_2;
1513
1514 p.z = t * step - thickness_2;
1515 if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error)
1516 p.z = thickness_2;
1517 }
1518 }
1519 }
1520
1521 return c;
1522}
1523
1524TabletopObjectsThread::CloudPtr
1525TabletopObjectsThread::generate_table_model(const float length,
1526 const float width,
1527 const float step,
1528 const float max_error)
1529{
1530 CloudPtr c(new Cloud());
1531
1532 const float length_2 = fabs(length) * 0.5;
1533 const float width_2 = fabs(width) * 0.5;
1534
1535 // calculate table points
1536 const unsigned int l_base_num = std::max(2u, (unsigned int)floor(length / step));
1537 const unsigned int num_w =
1538 l_base_num
1539 + ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1540 const unsigned int w_base_num = std::max(2u, (unsigned int)floor(width / step));
1541 const unsigned int num_h =
1542 w_base_num
1543 + ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1544
1545 //logger->log_debug(name(), "Generating table model %fx%f (%ux%u=%u points)",
1546 // length, width, num_w, num_h, num_w * num_h);
1547
1548 c->height = 1;
1549 c->width = num_w * num_h;
1550 c->is_dense = true;
1551 c->points.resize((size_t)num_w * (size_t)num_h);
1552
1553 unsigned int idx = 0;
1554 for (unsigned int w = 0; w < num_w; ++w) {
1555 for (unsigned int h = 0; h < num_h; ++h) {
1556 PointType &p = c->points[idx++];
1557
1558 p.x = h * step - width_2;
1559 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error)
1560 p.x = width_2;
1561
1562 p.y = w * step - length_2;
1563 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error)
1564 p.y = length_2;
1565
1566 p.z = 0.;
1567 }
1568 }
1569
1570 return c;
1571}
1572
1573TabletopObjectsThread::CloudPtr
1574TabletopObjectsThread::simplify_polygon(CloudPtr polygon, float dist_threshold)
1575{
1576 const float sqr_dist_threshold = dist_threshold * dist_threshold;
1577 CloudPtr result(new Cloud());
1578 const size_t psize = polygon->points.size();
1579 result->points.resize(psize);
1580 size_t res_points = 0;
1581 size_t i_dist = 1;
1582 for (size_t i = 1; i <= psize; ++i) {
1583 PointType &p1p = polygon->points[i - i_dist];
1584
1585 if (i == psize) {
1586 if (result->points.empty()) {
1587 // Simplification failed, got something too "line-ish"
1588 return polygon;
1589 }
1590 }
1591
1592 PointType &p2p = polygon->points[i % psize];
1593 PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize];
1594
1595 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0);
1596 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0);
1597 Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0);
1598
1599 Eigen::Vector4f line_dir = p3 - p1;
1600
1601 double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir);
1602 if (sqr_dist < sqr_dist_threshold) {
1603 ++i_dist;
1604 } else {
1605 i_dist = 1;
1606 result->points[res_points++] = p2p;
1607 }
1608 }
1609
1610 result->header.frame_id = polygon->header.frame_id;
1611 result->header.stamp = polygon->header.stamp;
1612 result->width = res_points;
1613 result->height = 1;
1614 result->is_dense = false;
1615 result->points.resize(res_points);
1616
1617 return result;
1618}
1619
1620void
1621TabletopObjectsThread::convert_colored_input()
1622{
1623 converted_input_->header.seq = colored_input_->header.seq;
1624 converted_input_->header.frame_id = colored_input_->header.frame_id;
1625 converted_input_->header.stamp = colored_input_->header.stamp;
1626 converted_input_->width = colored_input_->width;
1627 converted_input_->height = colored_input_->height;
1628 converted_input_->is_dense = colored_input_->is_dense;
1629
1630 const size_t size = colored_input_->points.size();
1631 converted_input_->points.resize(size);
1632 for (size_t i = 0; i < size; ++i) {
1633 const ColorPointType &in = colored_input_->points[i];
1634 PointType & out = converted_input_->points[i];
1635
1636 out.x = in.x;
1637 out.y = in.y;
1638 out.z = in.z;
1639 }
1640}
1641
1642void
1643TabletopObjectsThread::delete_old_centroids(OldCentroidVector centroids, unsigned int age)
1644{
1645 centroids.erase(std::remove_if(centroids.begin(),
1646 centroids.end(),
1647 [&](const OldCentroid &centroid) -> bool {
1648 if (centroid.get_age() > age) {
1649 free_ids_.push_back(centroid.get_id());
1650 return true;
1651 }
1652 return false;
1653 }),
1654 centroids.end());
1655}
1656
1657void
1658TabletopObjectsThread::delete_near_centroids(CentroidMap reference,
1659 OldCentroidVector centroids,
1660 float min_distance)
1661{
1662 centroids.erase(std::remove_if(centroids.begin(),
1663 centroids.end(),
1664 [&](const OldCentroid &old) -> bool {
1665 for (CentroidMap::const_iterator it = reference.begin();
1666 it != reference.end();
1667 ++it) {
1668 if (pcl::distances::l2(it->second, old.get_centroid())
1669 < min_distance) {
1670 free_ids_.push_back(old.get_id());
1671 return true;
1672 }
1673 }
1674 return false;
1675 }),
1676 centroids.end());
1677}
1678
1679void
1680TabletopObjectsThread::remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids)
1681{
1682 tf::Stamped<tf::Point> sp_baserel_table;
1683 tf::Stamped<tf::Point> sp_table(tf::Point(table_centroid[0],
1684 table_centroid[1],
1685 table_centroid[2]),
1686 fawkes::Time(0, 0),
1687 input_->header.frame_id);
1688 try {
1689 tf_listener->transform_point(cfg_base_frame_, sp_table, sp_baserel_table);
1690 for (CentroidMap::iterator it = centroids.begin(); it != centroids.end();) {
1691 try {
1692 tf::Stamped<tf::Point> sp_baserel_centroid(tf::Point(it->second[0],
1693 it->second[1],
1694 it->second[2]),
1695 fawkes::Time(0, 0),
1696 cfg_base_frame_);
1697 float d = sp_baserel_centroid.z() - sp_baserel_table.z();
1698 if (d > cfg_centroid_max_height_) {
1699 //logger->log_debug(name(), "remove centroid %u, too high (d=%f)", it->first, d);
1700 free_ids_.push_back(it->first);
1701 centroids.erase(it++);
1702 } else
1703 ++it;
1704 } catch (tf::TransformException &e) {
1705 // simply keep the centroid if we can't transform it
1706 ++it;
1707 }
1708 }
1709 } catch (tf::TransformException &e) {
1710 // keep all centroids if transformation of the table fails
1711 }
1712}
1713
1714Eigen::Vector4f
1715TabletopObjectsThread::fit_cylinder(ColorCloudConstPtr obj_in_base_frame,
1716 Eigen::Vector4f const &centroid,
1717 uint const & centroid_i)
1718{
1719 Eigen::Vector4f new_centroid(centroid);
1720 ColorPointType pnt_min, pnt_max;
1721 Eigen::Vector3f obj_dim;
1722 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> obj_size_scores;
1723 pcl::getMinMax3D(*obj_in_base_frame, pnt_min, pnt_max);
1724 obj_dim[0] = fabs(pnt_max.x - pnt_min.x);
1725 obj_dim[1] = fabs(pnt_max.y - pnt_min.y);
1726 obj_dim[2] = fabs(pnt_max.z - pnt_min.z);
1727 compute_bounding_box_scores(obj_dim, obj_size_scores);
1728
1730 name(), "Computed object dimensions: %f %f %f", obj_dim[0], obj_dim[1], obj_dim[2]);
1731 logger->log_debug(name(), "Size similarity to known objects:");
1732 for (int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1734 "** Cup %i: %f in x, %f in y, %f in z.",
1735 os,
1736 obj_size_scores[os][0],
1737 obj_size_scores[os][1],
1738 obj_size_scores[os][2]);
1739 obj_likelihoods_[centroid_i][os] =
1740 (double)obj_size_scores[os][0] * obj_size_scores[os][1] * obj_size_scores[os][2];
1741 }
1742
1743 //Fit cylinder:
1744 pcl::NormalEstimation<ColorPointType, pcl::Normal> ne;
1745 pcl::SACSegmentationFromNormals<ColorPointType, pcl::Normal> seg;
1746 pcl::ExtractIndices<ColorPointType> extract;
1747 pcl::ExtractIndices<pcl::Normal> extract_normals;
1749 pcl::search::KdTree<ColorPointType>::Ptr tree_cyl(new pcl::search::KdTree<ColorPointType>());
1750 pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
1751 pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
1752
1753 // Estimate point normals
1754 ne.setSearchMethod(tree_cyl);
1755 ne.setInputCloud(obj_in_base_frame);
1756 ne.setKSearch(10);
1757 ne.compute(*obj_normals);
1758
1759 ///////////////////////////////////////////////////////////////
1760 // Create the segmentation object for cylinder segmentation and set all the parameters
1761 seg.setOptimizeCoefficients(true);
1762 seg.setModelType(pcl::SACMODEL_CYLINDER);
1763 seg.setMethodType(pcl::SAC_RANSAC);
1764 seg.setNormalDistanceWeight(0.1);
1765 seg.setMaxIterations(1000);
1766 seg.setDistanceThreshold(0.07);
1767 seg.setRadiusLimits(0, 0.12);
1768
1769 seg.setInputCloud(obj_in_base_frame);
1770 seg.setInputNormals(obj_normals);
1771
1772 // Obtain the cylinder inliers and coefficients
1773
1774 seg.segment(*inliers_cylinder, *coefficients_cylinder);
1775 //std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
1776 //Getting max and min z values from cylinder inliers.
1777 extract.setInputCloud(obj_in_base_frame);
1778 extract.setIndices(inliers_cylinder);
1779 extract.setNegative(false);
1780 pcl::PointCloud<ColorPointType>::Ptr cloud_cylinder_baserel(
1782 extract.filter(*cloud_cylinder_baserel);
1783
1784 cylinder_params_[centroid_i][0] = 0;
1785 cylinder_params_[centroid_i][1] = 0;
1786 if (cloud_cylinder_baserel->points.empty()) {
1787 logger->log_debug(name(), "No cylinder inliers!!");
1788 obj_shape_confidence_[centroid_i] = 0.0;
1789 } else {
1790 if (!tf_listener->frame_exists(cloud_cylinder_baserel->header.frame_id)) {
1791 return centroid;
1792 }
1793
1794 obj_shape_confidence_[centroid_i] =
1795 (double)(cloud_cylinder_baserel->points.size()) / (obj_in_base_frame->points.size() * 1.0);
1797 "Cylinder fit confidence = %zu/%zu = %f",
1798 cloud_cylinder_baserel->points.size(),
1799 obj_in_base_frame->points.size(),
1800 obj_shape_confidence_[centroid_i]);
1801
1802 ColorPointType pnt_min;
1803 ColorPointType pnt_max;
1804 pcl::getMinMax3D(*cloud_cylinder_baserel, pnt_min, pnt_max);
1805 if (cfg_verbose_cylinder_fitting_) {
1807 "Cylinder height according to cylinder inliers: %f",
1808 pnt_max.z - pnt_min.z);
1809 logger->log_debug(name(), "Cylinder height according to bounding box: %f", obj_dim[2]);
1811 "Cylinder radius according to cylinder fitting: %f",
1812 (*coefficients_cylinder).values[6]);
1813 logger->log_debug(name(), "Cylinder radius according to bounding box y: %f", obj_dim[1] / 2);
1814 }
1815 //Cylinder radius:
1816 //cylinder_params_[centroid_i][0] = (*coefficients_cylinder).values[6];
1817 cylinder_params_[centroid_i][0] = obj_dim[1] / 2;
1818 //Cylinder height:
1819 //cylinder_params_[centroid_i][1] = (pnt_max->z - pnt_min->z);
1820 cylinder_params_[centroid_i][1] = obj_dim[2];
1821
1822 //cylinder_params_[centroid_i][2] = table_inclination_;
1823
1824 //Overriding computed centroids with estimated cylinder center:
1825 new_centroid[0] = pnt_min.x + 0.5 * (pnt_max.x - pnt_min.x);
1826 new_centroid[1] = pnt_min.y + 0.5 * (pnt_max.y - pnt_min.y);
1827 new_centroid[2] = pnt_min.z + 0.5 * (pnt_max.z - pnt_min.z);
1828 }
1829
1830 signed int detected_obj_id = -1;
1831 double best_confidence = 0.0;
1832 if (cfg_verbose_cylinder_fitting_) {
1833 logger->log_debug(name(), "Shape similarity = %f", obj_shape_confidence_[centroid_i]);
1834 }
1835 for (int os = 0; os < NUM_KNOWN_OBJS_; os++) {
1836 if (cfg_verbose_cylinder_fitting_) {
1837 logger->log_debug(name(), "** Similarity to known cup %i:", os);
1838 logger->log_debug(name(), "Size similarity = %f", obj_likelihoods_[centroid_i][os]);
1839 obj_likelihoods_[centroid_i][os] =
1840 (0.6 * obj_likelihoods_[centroid_i][os]) + (0.4 * obj_shape_confidence_[centroid_i]);
1841 logger->log_debug(name(), "Overall similarity = %f", obj_likelihoods_[centroid_i][os]);
1842 }
1843 if (obj_likelihoods_[centroid_i][os] > best_confidence) {
1844 best_confidence = obj_likelihoods_[centroid_i][os];
1845 detected_obj_id = os;
1846 }
1847 }
1848 if (cfg_verbose_cylinder_fitting_) {
1849 logger->log_debug(name(), "********************Object Result********************");
1850 }
1851 if (best_confidence > 0.6) {
1852 best_obj_guess_[centroid_i] = detected_obj_id;
1853
1854 if (cfg_verbose_cylinder_fitting_) {
1856 "MATCH FOUND!! -------------------------> Cup number %i",
1857 detected_obj_id);
1858 }
1859 } else {
1860 best_obj_guess_[centroid_i] = -1;
1861 if (cfg_verbose_cylinder_fitting_) {
1862 logger->log_debug(name(), "No match found.");
1863 }
1864 }
1865 if (cfg_verbose_cylinder_fitting_) {
1866 logger->log_debug(name(), "*****************************************************");
1867 }
1868
1869 return new_centroid;
1870}
1871
1872/**
1873 * calculate reassignment of IDs using the hungarian mehtod such that the total
1874 * distance all centroids moved is minimal
1875 * @param new_centroids current centroids which need correct ID assignment
1876 * @return map containing the new assignments, new_centroid -> ID
1877 * will be -1 if object is dropped, it's the caller's duty to remove any
1878 * reference to the centroid
1879 */
1880std::map<unsigned int, int>
1881TabletopObjectsThread::track_objects(
1882 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids)
1883{
1884 std::map<uint, int> final_assignment;
1885 if (first_run_) {
1886 // get a new id for every object since we didn't have objects before
1887 for (unsigned int i = 0; i < new_centroids.size(); i++) {
1888 final_assignment[i] = next_id();
1889 }
1890 return final_assignment;
1891 } else { // !first_run_
1892 TIMETRACK_START(ttc_hungarian_);
1893 hungarian_problem_t hp;
1894 // obj_ids: the id of the centroid in column i is saved in obj_ids[i]
1895 std::vector<unsigned int> obj_ids(centroids_.size());
1896 // create cost matrix,
1897 // save new centroids in rows, last centroids in columns
1898 // distance between new centroid i and last centroid j in cost[i][j]
1899 hp.num_rows = new_centroids.size();
1900 hp.num_cols = centroids_.size();
1901 hp.cost = (int **)calloc(hp.num_rows, sizeof(int *));
1902 for (int i = 0; i < hp.num_rows; i++)
1903 hp.cost[i] = (int *)calloc(hp.num_cols, sizeof(int));
1904 for (int row = 0; row < hp.num_rows; row++) { // new centroids
1905 unsigned int col = 0;
1906 for (CentroidMap::iterator col_it = centroids_.begin(); col_it != centroids_.end();
1907 ++col_it, ++col) { // old centroids
1908 double distance = pcl::distances::l2(new_centroids[row], col_it->second);
1909 hp.cost[row][col] = (int)(distance * 1000);
1910 obj_ids[col] = col_it->first;
1911 }
1912 }
1913 HungarianMethod solver;
1914 solver.init(hp.cost, hp.num_rows, hp.num_cols, HUNGARIAN_MODE_MINIMIZE_COST);
1915 solver.solve();
1916 // get assignments
1917 int assignment_size;
1918 int *assignment = solver.get_assignment(assignment_size);
1919 int id;
1920 for (int row = 0; row < assignment_size; row++) {
1921 if (row >= hp.num_rows) { // object has disappeared
1922 id = obj_ids.at(assignment[row]);
1923 old_centroids_.push_back(OldCentroid(id, centroids_.at(id)));
1924 continue;
1925 } else if (assignment[row] >= hp.num_cols) { // object is new or has reappeared
1926 bool assigned = false;
1927 // first, check if there is an old centroid close enough
1928 for (OldCentroidVector::iterator it = old_centroids_.begin(); it != old_centroids_.end();
1929 ++it) {
1930 if (pcl::distances::l2(new_centroids[row], it->get_centroid())
1931 <= cfg_centroid_max_distance_) {
1932 id = it->get_id();
1933 old_centroids_.erase(it);
1934 assigned = true;
1935 break;
1936 }
1937 }
1938 if (!assigned) {
1939 // we still don't have an id, create as new object
1940 id = next_id();
1941 }
1942 } else { // object has been assigned to an existing id
1943 id = obj_ids[assignment[row]];
1944 // check if centroid was moved further than cfg_centroid_max_distance_
1945 // this can happen if a centroid appears and another one disappears in the same loop
1946 // (then, the old centroid is assigned to the new one)
1947 if (pcl::distances::l2(centroids_[id], new_centroids[row]) > cfg_centroid_max_distance_) {
1948 // save the centroid because we don't use it now
1949 old_centroids_.push_back(OldCentroid(id, centroids_[id]));
1950 id = -1;
1951 }
1952 }
1953 final_assignment[row] = id;
1954 }
1955 return final_assignment;
1956 }
1957}
1958
1959#ifdef HAVE_VISUAL_DEBUGGING
1960void
1961TabletopObjectsThread::set_visualization_thread(TabletopVisualizationThreadBase *visthread)
1962{
1963 visthread_ = visthread;
1964}
1965#endif
This class is used to save old centroids in order to check for reappearance.
virtual void finalize()
Finalize the thread.
virtual ~TabletopObjectsThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Base class for virtualization thread.
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Hungarian method assignment solver.
Definition: hungarian.h:48
int * get_assignment(int &size)
Get assignment and size.
Definition: hungarian.cpp:571
int init(int **cost_matrix, int rows, int cols, int mode)
Initialize hungarian method.
Definition: hungarian.cpp:147
void solve()
Solve the assignment problem.
Definition: hungarian.cpp:248
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
void remove_pointcloud(const char *id)
Remove the point cloud.
bool exists_pointcloud(const char *id)
Check if point cloud exists.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Position3DInterface Fawkes BlackBoard Interface.
int32_t visibility_history() const
Get visibility_history value.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void set_frame(const char *new_frame)
Set frame value.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
void reset()
Reset pointer.
Definition: refptr.h:455
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
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
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Compare points' distance to a plane.
Definition: comparisons.h:101
pcl::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:108
Check if point is inside or outside a given polygon.
Definition: comparisons.h:45
pcl::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:52
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
Base class for fawkes tf exceptions.
Definition: exceptions.h:31
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
void transform_vector(const std::string &target_frame, const Stamped< Vector3 > &stamped_in, Stamped< Vector3 > &stamped_out) const
Transform a stamped vector into the target frame.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36