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