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