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