Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
tabletop_objects_thread.cpp
1 
2 /***************************************************************************
3  * tabletop_objects_thread.cpp - Thread to detect tabletop objects
4  *
5  * Created: Sat Nov 05 00:22:41 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "tabletop_objects_thread.h"
23 #include "cluster_colors.h"
24 #ifdef HAVE_VISUAL_DEBUGGING
25 # include "visualization_thread_base.h"
26 #endif
27 
28 #include <pcl_utils/utils.h>
29 #include <pcl_utils/comparisons.h>
30 #include <utils/time/wait.h>
31 #include <utils/math/angle.h>
32 #ifdef USE_TIMETRACKER
33 # include <utils/time/tracker.h>
34 #endif
35 
36 #include <pcl/point_cloud.h>
37 #include <pcl/point_types.h>
38 #include <pcl/sample_consensus/method_types.h>
39 #include <pcl/sample_consensus/model_types.h>
40 #include <pcl/segmentation/extract_clusters.h>
41 #include <pcl/filters/extract_indices.h>
42 #include <pcl/surface/convex_hull.h>
43 #include <pcl/kdtree/kdtree.h>
44 #include <pcl/kdtree/kdtree_flann.h>
45 #include <pcl/filters/project_inliers.h>
46 #include <pcl/filters/conditional_removal.h>
47 #include <pcl/common/centroid.h>
48 #include <pcl/common/transforms.h>
49 #include <pcl/common/distances.h>
50 
51 #include <interfaces/Position3DInterface.h>
52 #include <interfaces/SwitchInterface.h>
53 
54 #include <iostream>
55 using namespace std;
56 
57 #define CFG_PREFIX "/perception/tabletop-objects/"
58 
59 /** @class TabletopObjectsThread "tabletop_objects_thread.h"
60  * Main thread of tabletop objects plugin.
61  * @author Tim Niemueller
62  */
63 
64 using namespace fawkes;
65 
66 
67 #ifdef USE_TIMETRACKER
68 #define TIMETRACK_START(c) \
69  tt_->ping_start(c); \
70 
71 #define TIMETRACK_INTER(c1, c2) \
72  tt_->ping_end(c1); \
73  tt_->ping_start(c2);
74 
75 #define TIMETRACK_END(c) \
76  tt_->ping_end(c);
77 
78 #define TIMETRACK_ABORT(c) \
79  tt_->ping_abort(c);
80 
81 #else
82 
83 #define TIMETRACK_START(c)
84 #define TIMETRACK_INTER(c1, c2)
85 #define TIMETRACK_END(c)
86 #define TIMETRACK_ABORT(c)
87 #endif
88 
89 /** Constructor. */
91  : Thread("TabletopObjectsThread", Thread::OPMODE_CONTINUOUS),
92  TransformAspect(TransformAspect::ONLY_LISTENER)
93 {
94 #ifdef HAVE_VISUAL_DEBUGGING
95  visthread_ = NULL;
96 #endif
97 }
98 
99 
100 /** Destructor. */
102 {
103 }
104 
105 
106 void
108 {
109  cfg_depth_filter_min_x_ = config->get_float(CFG_PREFIX"depth_filter_min_x");
110  cfg_depth_filter_max_x_ = config->get_float(CFG_PREFIX"depth_filter_max_x");
111  cfg_voxel_leaf_size_ = config->get_float(CFG_PREFIX"voxel_leaf_size");
112  cfg_segm_max_iterations_ =
113  config->get_uint(CFG_PREFIX"table_segmentation_max_iterations");
114  cfg_segm_distance_threshold_ =
115  config->get_float(CFG_PREFIX"table_segmentation_distance_threshold");
116  cfg_segm_inlier_quota_ =
117  config->get_float(CFG_PREFIX"table_segmentation_inlier_quota");
118  cfg_max_z_angle_deviation_ = config->get_float(CFG_PREFIX"max_z_angle_deviation");
119  cfg_table_min_height_ = config->get_float(CFG_PREFIX"table_min_height");
120  cfg_table_max_height_ = config->get_float(CFG_PREFIX"table_max_height");
121  cfg_table_model_length_ = config->get_float(CFG_PREFIX"table_model_length");
122  cfg_table_model_width_ = config->get_float(CFG_PREFIX"table_model_width");
123  cfg_table_model_step_ = config->get_float(CFG_PREFIX"table_model_step");
124  cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX"horizontal_viewing_angle"));
125  cfg_vertical_va_ = deg2rad(config->get_float(CFG_PREFIX"vertical_viewing_angle"));
126  cfg_cluster_tolerance_ = config->get_float(CFG_PREFIX"cluster_tolerance");
127  cfg_cluster_min_size_ = config->get_uint(CFG_PREFIX"cluster_min_size");
128  cfg_cluster_max_size_ = config->get_uint(CFG_PREFIX"cluster_max_size");
129  cfg_result_frame_ = config->get_string(CFG_PREFIX"result_frame");
130 
131  finput_ = pcl_manager->get_pointcloud<PointType>("openni-pointcloud-xyz");
132  input_ = pcl_utils::cloudptr_from_refptr(finput_);
133 
134  try {
135  double rotation[4] = {0., 0., 0., 1.};
136  table_pos_if_ = NULL;
137  table_pos_if_ = blackboard->open_for_writing<Position3DInterface>("Tabletop");
138  table_pos_if_->set_rotation(rotation);
139  table_pos_if_->write();
140 
141  switch_if_ = NULL;
142  switch_if_ = blackboard->open_for_writing<SwitchInterface>("tabletop-objects");
143  switch_if_->set_enabled(true);
144  switch_if_->write();
145 
146  pos_ifs_.clear();
147  pos_ifs_.resize(MAX_CENTROIDS, NULL);
148  for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
149  char *tmp;
150  if (asprintf(&tmp, "Tabletop Object %u", i + 1) != -1) {
151  // Copy to get memory freed on exception
152  std::string id = tmp;
153  free(tmp);
154  Position3DInterface *iface =
156  pos_ifs_[i] = iface;
157  iface->set_rotation(rotation);
158  iface->write();
159  }
160  }
161  } catch (Exception &e) {
162  blackboard->close(table_pos_if_);
163  blackboard->close(switch_if_);
164  for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
165  if (pos_ifs_[i]) {
166  blackboard->close(pos_ifs_[i]);
167  }
168  }
169  pos_ifs_.clear();
170  throw;
171  }
172 
173  fclusters_ = new pcl::PointCloud<ColorPointType>();
174  fclusters_->header.frame_id = finput_->header.frame_id;
175  fclusters_->is_dense = false;
176  pcl_manager->add_pointcloud<ColorPointType>("tabletop-object-clusters", fclusters_);
177  clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
178 
179  ftable_model_ = new Cloud();
180  table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_);
181  table_model_->header.frame_id = finput_->header.frame_id;
182  pcl_manager->add_pointcloud("tabletop-table-model", ftable_model_);
183  pcl_utils::set_time(ftable_model_, fawkes::Time(clock));
184 
185  fsimplified_polygon_ = new Cloud();
186  simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_);
187  simplified_polygon_->header.frame_id = finput_->header.frame_id;
188  pcl_manager->add_pointcloud("tabletop-simplified-polygon", fsimplified_polygon_);
189  pcl_utils::set_time(fsimplified_polygon_, fawkes::Time(clock));
190 
191  grid_.setFilterFieldName("x");
192  grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_);
193  grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_);
194 
195  seg_.setOptimizeCoefficients(true);
196  seg_.setModelType(pcl::SACMODEL_PLANE);
197  seg_.setMethodType(pcl::SAC_RANSAC);
198  seg_.setMaxIterations(cfg_segm_max_iterations_);
199  seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
200 
201  loop_count_ = 0;
202 
203 #ifdef USE_TIMETRACKER
204  tt_ = new TimeTracker();
205  tt_loopcount_ = 0;
206  ttc_full_loop_ = tt_->add_class("Full Loop");
207  ttc_msgproc_ = tt_->add_class("Message Processing");
208  ttc_voxelize_ = tt_->add_class("Downsampling");
209  ttc_plane_ = tt_->add_class("Plane Segmentation");
210  ttc_extract_plane_ = tt_->add_class("Plane Extraction");
211  ttc_plane_downsampling_ = tt_->add_class("Plane Downsampling");
212  ttc_cluster_plane_ = tt_->add_class("Plane Clustering");
213  ttc_convex_hull_ = tt_->add_class("Convex Hull");
214  ttc_simplify_polygon_ = tt_->add_class("Polygon Simplification");
215  ttc_find_edge_ = tt_->add_class("Polygon Edge");
216  ttc_transform_ = tt_->add_class("Transform");
217  ttc_transform_model_ = tt_->add_class("Model Transformation");
218  ttc_extract_non_plane_ = tt_->add_class("Non-Plane Extraction");
219  ttc_polygon_filter_ = tt_->add_class("Polygon Filter");
220  ttc_table_to_output_ = tt_->add_class("Table to Output");
221  ttc_cluster_objects_ = tt_->add_class("Object Clustering");
222  ttc_visualization_ = tt_->add_class("Visualization");
223 #endif
224 }
225 
226 
227 void
229 {
230  input_.reset();
231  clusters_.reset();
232  simplified_polygon_.reset();
233 
234  pcl_manager->remove_pointcloud("tabletop-object-clusters");
235  pcl_manager->remove_pointcloud("tabletop-table-model");
236  pcl_manager->remove_pointcloud("tabletop-simplified-polygon");
237 
238  blackboard->close(table_pos_if_);
239  blackboard->close(switch_if_);
240  for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
241  blackboard->close(pos_ifs_[i]);
242  }
243  pos_ifs_.clear();
244 
245  finput_.reset();
246  fclusters_.reset();
247  ftable_model_.reset();
248  fsimplified_polygon_.reset();
249 }
250 
251 template <typename PointType>
252 inline bool
253 comparePoints2D(const PointType &p1, const PointType &p2)
254 {
255  double angle1 = atan2(p1.y, p1.x) + M_PI;
256  double angle2 = atan2(p2.y, p2.x) + M_PI;
257  return (angle1 > angle2);
258 }
259 
260 
261 // Criteria for *not* choosing a segment:
262 // 1. the existing current best is clearly closer in base-relative X direction
263 // 2. the existing current best is longer
264 bool
265 TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p,
266  PointType &br_p1p, PointType &br_p2p)
267 {
268  // current best base-relative points
269  Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z);
270  Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z);
271  Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5;
272 
273  Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z);
274  Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z);
275  Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5;
276 
277  double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]);
278 
279  // Criteria for *not* choosing a segment:
280  // 1. the existing current best is clearly closer in base-relative X direction
281  // 2. the existing current best is longer
282  if ( (dist_x < -0.25) ||
283  ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())) )
284  return false;
285  else
286  return true;
287 }
288 
289 
290 void
292 {
293  TIMETRACK_START(ttc_full_loop_);
294 
295  ++loop_count_;
296 
297  TIMETRACK_START(ttc_msgproc_);
298 
299  while (! switch_if_->msgq_empty()) {
301  switch_if_->msgq_first_safe(msg))
302  {
303  switch_if_->set_enabled(true);
304  switch_if_->write();
305  } else if (SwitchInterface::DisableSwitchMessage *msg =
306  switch_if_->msgq_first_safe(msg))
307  {
308  switch_if_->set_enabled(false);
309  switch_if_->write();
310  }
311 
312  switch_if_->msgq_pop();
313  }
314 
315  if (! switch_if_->is_enabled()) {
316  TimeWait::wait(250000);
317  return;
318  }
319 
320  TIMETRACK_INTER(ttc_msgproc_, ttc_voxelize_)
321 
322  CloudPtr temp_cloud(new Cloud);
323  CloudPtr temp_cloud2(new Cloud);
324  pcl::ExtractIndices<PointType> extract_;
325  CloudPtr cloud_hull_;
326  CloudPtr model_cloud_hull_;
327  CloudPtr cloud_proj_;
328  CloudPtr cloud_filt_;
329  CloudPtr cloud_above_;
330  CloudPtr cloud_objs_;
331  pcl::search::KdTree<PointType> kdtree_;
332 
333  grid_.setInputCloud(input_);
334  grid_.filter(*temp_cloud);
335 
336  if (temp_cloud->points.size() <= 10) {
337  // this can happen if run at startup. Since tabletop threads runs continuous
338  // and not synchronized with main loop, but point cloud acquisition thread is
339  // synchronized, we might start before any data has been read
340  //logger->log_warn(name(), "Empty voxelized point cloud, omitting loop");
341  TimeWait::wait(50000);
342  return;
343  }
344 
345  TIMETRACK_INTER(ttc_voxelize_, ttc_plane_)
346 
347  pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients());
348  pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
349  Eigen::Vector4f table_centroid, baserel_table_centroid(0,0,0,0);
350 
351  // This will search for the first plane which:
352  // 1. has a considerable amount of points (>= some percentage of input points)
353  // 2. is parallel to the floor (transformed normal angle to Z axis in specified epsilon)
354  // 3. is on a typical table height (at a specified height range in robot frame)
355  // Planes found along the way not satisfying any of the criteria are removed,
356  // the first plane either satisfying all criteria, or violating the first
357  // one end the loop
358  bool happy_with_plane = false;
359  while (! happy_with_plane) {
360  happy_with_plane = true;
361 
362  if (temp_cloud->points.size() <= 10) {
363  logger->log_warn(name(), "[L %u] no more points for plane detection, skipping loop", loop_count_);
364  set_position(table_pos_if_, false);
365  TIMETRACK_ABORT(ttc_plane_);
366  TIMETRACK_ABORT(ttc_full_loop_);
367  TimeWait::wait(50000);
368  return;
369  }
370 
371  seg_.setInputCloud(temp_cloud);
372  seg_.segment(*inliers, *coeff);
373 
374  // 1. check for a minimum number of expected inliers
375  if ((double)inliers->indices.size() < (cfg_segm_inlier_quota_ * (double)temp_cloud->points.size())) {
376  logger->log_warn(name(), "[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)",
377  loop_count_, inliers->indices.size(),
378  (cfg_segm_inlier_quota_ * temp_cloud->points.size()), temp_cloud->points.size());
379  set_position(table_pos_if_, false);
380  TIMETRACK_ABORT(ttc_plane_);
381  TIMETRACK_ABORT(ttc_full_loop_);
382  TimeWait::wait(50000);
383  return;
384  }
385 
386  // 2. Check angle between normal vector and Z axis of the
387  // base_link robot frame since tables are usually parallel to the ground...
388  try {
390  table_normal(tf::Vector3(coeff->values[0], coeff->values[1], coeff->values[2]),
391  fawkes::Time(0,0), input_->header.frame_id);
392 
393  tf::Stamped<tf::Vector3> baserel_normal;
394  tf_listener->transform_vector("/base_link", table_normal, baserel_normal);
395  tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z()));
396 
397  if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_ ) {
398  happy_with_plane = false;
399  logger->log_warn(name(), "[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding",
400  loop_count_, baserel_normal.x(), baserel_normal.y(), baserel_normal.z(),
401  z_axis.angle(baserel_normal), cfg_max_z_angle_deviation_);
402  }
403  } catch (tf::TransformException &e) {
404  //logger->log_warn(name(), "Transforming normal failed, exception follows");
405  //logger->log_warn(name(), e);
406  }
407 
408  if (happy_with_plane) {
409  // ok so far
410 
411  // 3. Calculate table centroid, then transform it to the base_link system
412  // to make a table height sanity check, they tend to be at a specific height...
413  try {
414  pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid);
416  centroid(tf::Point(table_centroid[0], table_centroid[1], table_centroid[2]),
417  fawkes::Time(0, 0), input_->header.frame_id);
418  tf::Stamped<tf::Point> baserel_centroid;
419  tf_listener->transform_point("/base_link", centroid, baserel_centroid);
420  baserel_table_centroid[0] = baserel_centroid.x();
421  baserel_table_centroid[1] = baserel_centroid.y();
422  baserel_table_centroid[2] = baserel_centroid.z();
423 
424  if ((baserel_centroid.z() < cfg_table_min_height_) ||
425  (baserel_centroid.z() > cfg_table_max_height_))
426  {
427  happy_with_plane = false;
428  logger->log_warn(name(), "[L %u] table height %f not in range [%f, %f]",
429  loop_count_, baserel_centroid.z(),
430  cfg_table_min_height_, cfg_table_max_height_);
431  }
432  } catch (tf::TransformException &e) {
433  //logger->log_warn(name(), "Transforming centroid failed, exception follows");
434  //logger->log_warn(name(), e);
435  }
436  }
437 
438 
439  if (! happy_with_plane) {
440  // throw away
441  Cloud extracted;
442  extract_.setNegative(true);
443  extract_.setInputCloud(temp_cloud);
444  extract_.setIndices(inliers);
445  extract_.filter(extracted);
446  *temp_cloud = extracted;
447  }
448  }
449 
450  // If we got here we found the table
451  // Do NOT set it here, we will still try to determine the rotation as well
452  // set_position(table_pos_if_, true, table_centroid);
453 
454  TIMETRACK_INTER(ttc_plane_, ttc_extract_plane_)
455 
456  extract_.setNegative(false);
457  extract_.setInputCloud(temp_cloud);
458  extract_.setIndices(inliers);
459  extract_.filter(*temp_cloud2);
460 
461 
462  // Project the model inliers
463  pcl::ProjectInliers<PointType> proj;
464  proj.setModelType(pcl::SACMODEL_PLANE);
465  proj.setInputCloud(temp_cloud2);
466  proj.setModelCoefficients(coeff);
467  cloud_proj_.reset(new Cloud());
468  proj.filter(*cloud_proj_);
469 
470  TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_);
471 
472  // ***
473  // In the following cluster the projected table plane. This is done to get
474  // the largest continuous part of the plane to remove outliers, for instance
475  // if the intersection of the plane with a wall or object is taken into the
476  // table points.
477  // To achieve this cluster, if an acceptable cluster was found, extract this
478  // cluster as the new table points. Otherwise continue with the existing
479  // point cloud.
480 
481  // further downsample table
482  CloudPtr cloud_table_voxelized(new Cloud());
483  pcl::VoxelGrid<PointType> table_grid;
484  table_grid.setLeafSize(0.04, 0.04, 0.04);
485  table_grid.setInputCloud(cloud_proj_);
486  table_grid.filter(*cloud_table_voxelized);
487 
488  TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_);
489 
490  // Creating the KdTree object for the search method of the extraction
491  pcl::search::KdTree<PointType>::Ptr
492  kdtree_table(new pcl::search::KdTree<PointType>());
493  kdtree_table->setInputCloud(cloud_table_voxelized);
494 
495  std::vector<pcl::PointIndices> table_cluster_indices;
496  pcl::EuclideanClusterExtraction<PointType> table_ec;
497  table_ec.setClusterTolerance(0.044);
498  table_ec.setMinClusterSize(0.8 * cloud_table_voxelized->points.size());
499  table_ec.setMaxClusterSize(cloud_table_voxelized->points.size());
500  table_ec.setSearchMethod(kdtree_table);
501  table_ec.setInputCloud(cloud_table_voxelized);
502  table_ec.extract(table_cluster_indices);
503 
504  if (! table_cluster_indices.empty()) {
505  // take the first, i.e. the largest cluster
506  CloudPtr cloud_table_extracted(new Cloud());
507  pcl::PointIndices::ConstPtr table_cluster_indices_ptr(new pcl::PointIndices(table_cluster_indices[0]));
508  pcl::ExtractIndices<PointType> table_cluster_extract;
509  table_cluster_extract.setNegative(false);
510  table_cluster_extract.setInputCloud(cloud_table_voxelized);
511  table_cluster_extract.setIndices(table_cluster_indices_ptr);
512  table_cluster_extract.filter(*cloud_table_extracted);
513  *cloud_proj_ = *cloud_table_extracted;
514  } else {
515  // Don't mess with the table, clustering didn't help to make it any better
516  logger->log_info(name(), "[L %u] table plane clustering did not generate any clusters", loop_count_);
517  }
518 
519  TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_)
520 
521 
522  // Estimate 3D convex hull -> TABLE BOUNDARIES
523  pcl::ConvexHull<PointType> hr;
524  //hr.setAlpha(0.1); // only for ConcaveHull
525  hr.setInputCloud(cloud_proj_);
526  cloud_hull_.reset(new Cloud());
527  hr.reconstruct(*cloud_hull_);
528 
529 
530  if (cloud_hull_->points.empty()) {
531  logger->log_warn(name(), "[L %u] convex hull of table empty, skipping loop", loop_count_);
532  TIMETRACK_ABORT(ttc_convex_hull_);
533  TIMETRACK_ABORT(ttc_full_loop_);
534  set_position(table_pos_if_, false);
535  return;
536  }
537 
538  TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_)
539 
540  CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02);
541  *simplified_polygon_ = *simplified_polygon;
542  //logger->log_debug(name(), "Original polygon: %zu simplified: %zu", cloud_hull_->points.size(),
543  // simplified_polygon->points.size());
544  *cloud_hull_ = *simplified_polygon;
545 
546  TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_)
547 
548 #ifdef HAVE_VISUAL_DEBUGGING
550  good_hull_edges.resize(cloud_hull_->points.size() * 2);
551 #endif
552 
553  try {
554 
555  // Get transform Input camera -> base_link
557  fawkes::Time input_time(0,0);
558  //pcl_utils::get_time(finput_, input_time);
559  tf_listener->lookup_transform("/base_link", finput_->header.frame_id,
560  input_time, t);
561 
562  tf::Quaternion q = t.getRotation();
563  Eigen::Affine3f affine_cloud =
564  Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z())
565  * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z());
566 
567  // Transform polygon cloud into base_link frame
568  CloudPtr baserel_polygon_cloud(new Cloud());
569  pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud);
570 
571  // Setup plane normals for left, right, and lower frustrum
572  // planes for line segment verification
573  Eigen::Vector3f left_frustrum_normal =
574  Eigen::AngleAxisf( cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
575  * (-1. * Eigen::Vector3f::UnitY());
576 
577  Eigen::Vector3f right_frustrum_normal =
578  Eigen::AngleAxisf(- cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ())
579  * Eigen::Vector3f::UnitY();
580 
581  Eigen::Vector3f lower_frustrum_normal =
582  Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY())
583  * Eigen::Vector3f::UnitZ();
584 
585  // point and good edge indexes of chosen candidate
586  size_t pidx1, pidx2;
587 #ifdef HAVE_VISUAL_DEBUGGING
588  size_t geidx1 = std::numeric_limits<size_t>::max();
589  size_t geidx2 = std::numeric_limits<size_t>::max();
590 #endif
591  // lower frustrum potential candidate
592  size_t lf_pidx1, lf_pidx2;
593  pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max();
594 
595  // Search for good edges and backup edge candidates
596  // Good edges are the ones with either points close to
597  // two separate frustrum planes, and hence the actual line is
598  // well inside the frustrum, or with points inside the frustrum.
599  // Possible backup candidates are lines with both points
600  // close to the lower frustrum plane, i.e. lines cutoff by the
601  // vertical viewing angle. If we cannot determine a suitable edge
602  // otherwise we fallback to this line as it is a good rough guess
603  // to prevent at least worst things during manipulation
604  const size_t psize = cloud_hull_->points.size();
605 #ifdef HAVE_VISUAL_DEBUGGING
606  size_t good_edge_points = 0;
607 #endif
608  for (size_t i = 0; i < psize; ++i) {
609  //logger->log_debug(name(), "Checking %zu and %zu of %zu", i, (i+1) % psize, psize);
610  PointType &p1p = cloud_hull_->points[i ];
611  PointType &p2p = cloud_hull_->points[(i+1) % psize];
612 
613  Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
614  Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
615 
616  PointType &br_p1p = baserel_polygon_cloud->points[i ];
617  PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize];
618 
619  // check if both end points are close to left or right frustrum plane
620  if ( ! (((left_frustrum_normal.dot(p1) < 0.03) &&
621  (left_frustrum_normal.dot(p2) < 0.03)) ||
622  ((right_frustrum_normal.dot(p1) < 0.03) &&
623  (right_frustrum_normal.dot(p2) < 0.03)) ) )
624  {
625  // candidate edge, i.e. it's not too close to left or right frustrum planes
626 
627  // check if both end points close to lower frustrum plane
628  if ((lower_frustrum_normal.dot(p1) < 0.01) &&
629  (lower_frustrum_normal.dot(p2) < 0.01)) {
630  // it's a lower frustrum line, keep just in case we do not
631  // find a better one
632  if ( (lf_pidx1 == std::numeric_limits<size_t>::max()) ||
633  is_polygon_edge_better(br_p1p, br_p2p,
634  baserel_polygon_cloud->points[lf_pidx1], baserel_polygon_cloud->points[lf_pidx2]))
635  {
636  // there was no backup candidate, yet, or this one is closer
637  // to the robot, take it.
638  lf_pidx1 = i;
639  lf_pidx2 = (i + 1) % psize;
640  }
641 
642  continue;
643  }
644 
645 #ifdef HAVE_VISUAL_DEBUGGING
646  // Remember as good edge for visualization
647  for (unsigned int j = 0; j < 3; ++j)
648  good_hull_edges[good_edge_points][j] = p1[j];
649  good_hull_edges[good_edge_points][3] = 0.;
650  ++good_edge_points;
651  for (unsigned int j = 0; j < 3; ++j)
652  good_hull_edges[good_edge_points][j] = p2[j];
653  good_hull_edges[good_edge_points][3] = 0.;
654  ++good_edge_points;
655 #endif
656 
657  if (pidx1 != std::numeric_limits<size_t>::max()) {
658  // current best base-relative points
659  PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1];
660  PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2];
661 
662  if (! is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p))
663  {
664  //logger->log_info(name(), "Skipping: cb(%f,%f)->(%f,%f) c(%f,%f)->(%f,%f)",
665  // cb_br_p1p.x, cb_br_p1p.y, cb_br_p2p.x, cb_br_p2p.y,
666  // br_p1p.x, br_p1p.y, br_p2p.x, br_p2p.y);
667  continue;
668  } else {
669  //logger->log_info(name(), "Taking: cb(%f,%f)->(%f,%f) c(%f,%f)->(%f,%f)",
670  // cb_br_p1p.x, cb_br_p1p.y, cb_br_p2p.x, cb_br_p2p.y,
671  // br_p1p.x, br_p1p.y, br_p2p.x, br_p2p.y);
672  }
673  //} else {
674  //logger->log_info(name(), "Taking because we had none");
675  }
676 
677  // Was not sorted out, therefore promote candidate to current best
678  pidx1 = i;
679  pidx2 = (i + 1) % psize;
680 #ifdef HAVE_VISUAL_DEBUGGING
681  geidx1 = good_edge_points - 2;
682  geidx2 = good_edge_points - 1;
683 #endif
684  }
685  }
686 
687  //logger->log_debug(name(), "Current best is %zu -> %zu", pidx1, pidx2);
688 
689  // in the case we have a backup lower frustrum edge check if we should use it
690  // Criteria:
691  // 0. we have a backup point
692  // 1. no other suitable edge was chosen at all
693  // 2. angle(Y_axis, chosen_edge) > threshold
694  // 3.. p1.x or p2.x > centroid.x
695  if (lf_pidx1 != std::numeric_limits<size_t>::max()) {
696  PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1];
697  PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2];
698 
699  Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.);
700  Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.);
701 
702  // None found at all
703  if (pidx1 == std::numeric_limits<size_t>::max()) {
704  pidx1 = lf_pidx1;
705  pidx2 = lf_pidx2;
706 
707 #ifdef HAVE_VISUAL_DEBUGGING
708  good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
709  good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
710  good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
711  geidx1 = good_edge_points++;
712 
713  good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
714  good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
715  good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
716  geidx2 = good_edge_points++;
717 #endif
718 
719  } else {
720 
721  PointType &p1p = baserel_polygon_cloud->points[pidx1];
722  PointType &p2p = baserel_polygon_cloud->points[pidx2];
723 
724  Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.);
725  Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.);
726 
727  // Unsuitable "good" line until now?
728  if (//(pcl::getAngle3D(p2 - p1, Eigen::Vector4f::UnitZ()) > M_PI * 0.5) ||
729  (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0]))
730  {
731  //logger->log_warn(name(), "Choosing backup candidate!");
732 
733  pidx1 = lf_pidx1;
734  pidx2 = lf_pidx2;
735 
736 #ifdef HAVE_VISUAL_DEBUGGING
737  good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x;
738  good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y;
739  good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z;
740  geidx1 = good_edge_points++;
741 
742  good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x;
743  good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y;
744  good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z;
745  geidx2 = good_edge_points++;
746 #endif
747  }
748  }
749  }
750 
751  //logger->log_info(name(), "Chose %zu -> %zu", pidx1, pidx2);
752 
753 #ifdef HAVE_VISUAL_DEBUGGING
754  if (good_edge_points > 0) {
755  good_hull_edges[geidx1][3] = 1.0;
756  good_hull_edges[geidx2][3] = 1.0;
757  }
758  good_hull_edges.resize(good_edge_points);
759 #endif
760 
761  TIMETRACK_INTER(ttc_find_edge_, ttc_transform_)
762 
763  // Calculate transformation parameters based on determined
764  // convex hull polygon segment we decided on as "the table edge"
765  PointType &p1p = cloud_hull_->points[pidx1];
766  PointType &p2p = cloud_hull_->points[pidx2];
767 
768  Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z);
769  Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z);
770 
771  // Normal vectors for table model and plane
772  Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ();
773  Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]);
774  normal.normalize(); // just in case
775 
776  Eigen::Vector3f table_centroid_3f =
777  Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]);
778 
779  // Rotational parameters to align table to polygon segment
780  Eigen::Vector3f p1_p2 = p2 - p1;
781  Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5;
782  p1_p2.normalize();
783  Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal);
784  p1_p2_normal_cross.normalize();
785 
786  // For N=(A,B,C), and hessian Ax+By+Cz+D=0 and N dot X=(Ax+By+Cz)
787  // we get N dot X + D = 0 -> -D = N dot X
788  double nD = - p1_p2_normal_cross.dot(p1_p2_center);
789  double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD;
790  if (p1_p2_centroid_dist < 0) {
791  // normal points to the "wrong" side fo our purpose
792  p1_p2_normal_cross *= -1;
793  }
794 
795  Eigen::Vector3f table_center =
796  p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5);
797 
798  for (unsigned int i = 0; i < 3; ++i) table_centroid[i] = table_center[i];
799  table_centroid[3] = 0.;
800 
801  // calculate table corner points
802  std::vector<Eigen::Vector3f> tpoints(4);
803  tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5);
804  tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_;
805  tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5);
806  tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_;
807 
808  model_cloud_hull_.reset(new Cloud());
809  model_cloud_hull_->points.resize(4);
810  model_cloud_hull_->height = 1;
811  model_cloud_hull_->width = 4;
812  model_cloud_hull_->is_dense = true;
813  for (int i = 0; i < 4; ++i) {
814  model_cloud_hull_->points[i].x = tpoints[i][0];
815  model_cloud_hull_->points[i].y = tpoints[i][1];
816  model_cloud_hull_->points[i].z = tpoints[i][2];
817  }
818  //std::sort(model_cloud_hull_->points.begin(),
819  // model_cloud_hull_->points.end(), comparePoints2D<PointType>);
820 
821  // Rotational parameters to rotate table model from camera to
822  // determined table position in 3D space
823  Eigen::Vector3f rotaxis = model_normal.cross(normal);
824  rotaxis.normalize();
825  double angle = acos(normal.dot(model_normal));
826 
827  // Transformation to translate model from camera center into actual pose
828  Eigen::Affine3f affine =
829  Eigen::Translation3f(table_centroid.x(), table_centroid.y(),
830  table_centroid.z())
831  * Eigen::AngleAxisf(angle, rotaxis);
832 
833  Eigen::Vector3f
834  model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.),
835  model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.);
836  model_p1 = affine * model_p1;
837  model_p2 = affine * model_p2;
838 
839  // Calculate the vector between model_p1 and model_p2
840  Eigen::Vector3f model_p1_p2 = model_p2 - model_p1;
841  model_p1_p2.normalize();
842  // Calculate rotation axis between model_p1 and model_p2
843  Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2);
844  model_rotaxis.normalize();
845  double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2));
846  //logger->log_info(name(), "Angle: %f Poly (%f,%f,%f) -> (%f,%f,%f) model (%f,%f,%f) -> (%f,%f,%f)",
847  // angle_p1_p2, p1.x(), p1.y(), p1.z(), p2.x(), p2.y(), p2.z(),
848  // model_p1.x(), model_p1.y(), model_p1.z(), model_p2.x(), model_p2.y(), model_p2.z());
849 
850  // Final full transformation of the table within the camera coordinate frame
851  affine =
852  Eigen::Translation3f(table_centroid.x(), table_centroid.y(),
853  table_centroid.z())
854  * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis)
855  * Eigen::AngleAxisf(angle, rotaxis);
856 
857 
858  // Just the rotational part
859  Eigen::Quaternionf qt;
860  qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis)
861  * Eigen::AngleAxisf(angle, rotaxis);
862 
863  // Set position again, this time with the rotation
864  set_position(table_pos_if_, true, table_centroid, qt);
865 
866  TIMETRACK_INTER(ttc_transform_, ttc_transform_model_)
867 
868  // to show fitted table model
869  CloudPtr table_model = generate_table_model(cfg_table_model_length_, cfg_table_model_width_, cfg_table_model_step_);
870  pcl::transformPointCloud(*table_model, *table_model_, affine.matrix());
871  //*table_model_ = *model_cloud_hull_;
872  //*table_model_ = *table_model;
873  table_model_->header.frame_id = finput_->header.frame_id;
874 
875  TIMETRACK_END(ttc_transform_model_);
876 
877  } catch (Exception &e) {
878  set_position(table_pos_if_, false);
879  logger->log_warn(name(), "Failed to transform convex hull cloud, exception follows");
880  logger->log_warn(name(), e);
881  TIMETRACK_ABORT(ttc_find_edge_);
882  }
883 
884  TIMETRACK_START(ttc_extract_non_plane_);
885  // Extract all non-plane points
886  cloud_filt_.reset(new Cloud());
887  extract_.setNegative(true);
888  extract_.filter(*cloud_filt_);
889 
890  TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_);
891 
892  // Use only points above tables
893  // Why coeff->values[3] > 0 ? ComparisonOps::GT : ComparisonOps::LT?
894  // The model coefficients are in Hessian Normal Form, hence coeff[0..2] are
895  // the normal vector. We need to distinguish the cases where the normal vector
896  // points towards the origin (camera) or away from it. This can be checked
897  // by calculating the distance towards the origin, which conveniently in
898  // dist = N * x + p is just p which is coeff[3]. Therefore, if coeff[3] is
899  // positive, the normal vector points towards the camera and we want all
900  // points with positive distance from the table plane, otherwise it points
901  // away from the origin and we want points with "negative distance".
902  // We make use of the fact that we only have a boring RGB-D camera and
903  // not an X-Ray...
904  pcl::ComparisonOps::CompareOp op =
905  coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT;
907  above_comp(new pcl_utils::PlaneDistanceComparison<PointType>(coeff, op));
908  pcl::ConditionAnd<PointType>::Ptr
909  above_cond(new pcl::ConditionAnd<PointType>());
910  above_cond->addComparison(above_comp);
911  pcl::ConditionalRemoval<PointType> above_condrem(above_cond);
912  above_condrem.setInputCloud(cloud_filt_);
913  //above_condrem.setKeepOrganized(true);
914  cloud_above_.reset(new Cloud());
915  above_condrem.filter(*cloud_above_);
916 
917  //printf("Before: %zu After: %zu\n", cloud_filt_->points.size(),
918  // cloud_above_->points.size());
919  if (cloud_filt_->points.size() < cfg_cluster_min_size_) {
920  //logger->log_warn(name(), "Less points than cluster min size");
921  TIMETRACK_ABORT(ttc_polygon_filter_);
922  TIMETRACK_ABORT(ttc_full_loop_);
923  return;
924  }
925 
926  // Extract only points on the table plane
927  pcl::PointIndices::Ptr polygon(new pcl::PointIndices());
928 
929  pcl::ConditionAnd<PointType>::Ptr
930  polygon_cond(new pcl::ConditionAnd<PointType>());
931 
934  (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) ? *model_cloud_hull_ : *cloud_hull_));
935  polygon_cond->addComparison(inpoly_comp);
936 
937  // build the filter
938  pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
939  condrem.setInputCloud(cloud_above_);
940  //condrem.setKeepOrganized(true);
941  cloud_objs_.reset(new Cloud());
942  condrem.filter(*cloud_objs_);
943 
944  //CloudPtr table_points(new Cloud());
945  //condrem.setInputCloud(temp_cloud2);
946  //condrem.filter(*table_points);
947 
948  // CLUSTERS
949  // extract clusters of OBJECTS
950 
951  TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_)
952 
953  ColorCloudPtr tmp_clusters(new ColorCloud());
954  tmp_clusters->header.frame_id = clusters_->header.frame_id;
955  std::vector<int> &indices = inliers->indices;
956  tmp_clusters->height = 1;
957  //const size_t tsize = table_points->points.size();
958  //tmp_clusters->width = tsize;
959  //tmp_clusters->points.resize(tsize);
960  tmp_clusters->width = indices.size();
961  tmp_clusters->points.resize(indices.size());
962  for (size_t i = 0; i < indices.size(); ++i) {
963  PointType &p1 = temp_cloud2->points[i];
964  //PointType &p1 = table_points->points[i];
965  ColorPointType &p2 = tmp_clusters->points[i];
966  p2.x = p1.x;
967  p2.y = p1.y;
968  p2.z = p1.z;
969  p2.r = table_color[0];
970  p2.g = table_color[1];
971  p2.b = table_color[2];
972  }
973 
974  TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_)
975 
976  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
977  centroids.resize(MAX_CENTROIDS);
978  unsigned int centroid_i = 0;
979 
980  if (cloud_objs_->points.size() > 0) {
981  // Creating the KdTree object for the search method of the extraction
982  pcl::search::KdTree<PointType>::Ptr
983  kdtree_cl(new pcl::search::KdTree<PointType>());
984  kdtree_cl->setInputCloud(cloud_objs_);
985 
986  std::vector<pcl::PointIndices> cluster_indices;
987  pcl::EuclideanClusterExtraction<PointType> ec;
988  ec.setClusterTolerance(cfg_cluster_tolerance_);
989  ec.setMinClusterSize(cfg_cluster_min_size_);
990  ec.setMaxClusterSize(cfg_cluster_max_size_);
991  ec.setSearchMethod(kdtree_cl);
992  ec.setInputCloud(cloud_objs_);
993  ec.extract(cluster_indices);
994 
995  //logger->log_debug(name(), "Found %zu clusters", cluster_indices.size());
996 
997  ColorCloudPtr colored_clusters(new ColorCloud());
998  colored_clusters->header.frame_id = clusters_->header.frame_id;
999  std::vector<pcl::PointIndices>::const_iterator it;
1000  unsigned int cci = 0;
1001  //unsigned int i = 0;
1002  unsigned int num_points = 0;
1003  for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
1004  num_points += it->indices.size();
1005 
1006  if (num_points > 0) {
1007  colored_clusters->points.resize(num_points);
1008  for (it = cluster_indices.begin();
1009  it != cluster_indices.end() && centroid_i < MAX_CENTROIDS;
1010  ++it, ++centroid_i)
1011  {
1012  pcl::compute3DCentroid(*cloud_objs_, it->indices, centroids[centroid_i]);
1013 
1014  std::vector<int>::const_iterator pit;
1015  for (pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
1016  ColorPointType &p1 = colored_clusters->points[cci++];
1017  PointType &p2 = cloud_objs_->points[*pit];
1018  p1.x = p2.x;
1019  p1.y = p2.y;
1020  p1.z = p2.z;
1021  p1.r = cluster_colors[centroid_i][0];
1022  p1.g = cluster_colors[centroid_i][1];;
1023  p1.b = cluster_colors[centroid_i][2];;
1024  }
1025  }
1026 
1027  *tmp_clusters += *colored_clusters;
1028  } else {
1029  logger->log_info(name(), "No clustered points found");
1030  }
1031  } else {
1032  logger->log_info(name(), "Filter left no points for clustering");
1033  }
1034 
1035  for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) {
1036  set_position(pos_ifs_[i], i < centroid_i, centroids[i]);
1037  }
1038  centroids.resize(centroid_i);
1039 
1040  TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_)
1041 
1042  *clusters_ = *tmp_clusters;
1043  pcl_utils::copy_time(finput_, fclusters_);
1044  pcl_utils::copy_time(finput_, ftable_model_);
1045  pcl_utils::copy_time(finput_, fsimplified_polygon_);
1046 
1047 #ifdef HAVE_VISUAL_DEBUGGING
1048  if (visthread_) {
1049  Eigen::Vector4f normal;
1050  normal[0] = coeff->values[0];
1051  normal[1] = coeff->values[1];
1052  normal[2] = coeff->values[2];
1053  normal[3] = 0.;
1054 
1056  hull_vertices.resize(cloud_hull_->points.size());
1057  for (unsigned int i = 0; i < cloud_hull_->points.size(); ++i) {
1058  hull_vertices[i][0] = cloud_hull_->points[i].x;
1059  hull_vertices[i][1] = cloud_hull_->points[i].y;
1060  hull_vertices[i][2] = cloud_hull_->points[i].z;
1061  hull_vertices[i][3] = 0.;
1062  }
1063 
1065  if (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) {
1066  model_vertices.resize(model_cloud_hull_->points.size());
1067  for (unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) {
1068  model_vertices[i][0] = model_cloud_hull_->points[i].x;
1069  model_vertices[i][1] = model_cloud_hull_->points[i].y;
1070  model_vertices[i][2] = model_cloud_hull_->points[i].z;
1071  model_vertices[i][3] = 0.;
1072  }
1073  }
1074 
1075  visthread_->visualize(input_->header.frame_id,
1076  table_centroid, normal, hull_vertices, model_vertices,
1077  good_hull_edges, centroids);
1078  }
1079 #endif
1080 
1081  TIMETRACK_END(ttc_visualization_);
1082  TIMETRACK_END(ttc_full_loop_);
1083 
1084 #ifdef USE_TIMETRACKER
1085  if (++tt_loopcount_ >= 5) {
1086  tt_loopcount_ = 0;
1087  tt_->print_to_stdout();
1088  }
1089 #endif
1090 }
1091 
1092 
1093 void
1094 TabletopObjectsThread::set_position(fawkes::Position3DInterface *iface,
1095  bool is_visible, const Eigen::Vector4f &centroid,
1096  const Eigen::Quaternionf &attitude)
1097 {
1098  tf::Stamped<tf::Pose> baserel_pose;
1099  try{
1101  spose(tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
1102  tf::Vector3(centroid[0], centroid[1], centroid[2])),
1103  fawkes::Time(0, 0), input_->header.frame_id);
1104  tf_listener->transform_pose(cfg_result_frame_, spose, baserel_pose);
1105  iface->set_frame(cfg_result_frame_.c_str());
1106  } catch (tf::TransformException &e) {
1107  is_visible = false;
1108  }
1109 
1110  int visibility_history = iface->visibility_history();
1111  if (is_visible) {
1112  if (visibility_history >= 0) {
1113  iface->set_visibility_history(visibility_history + 1);
1114  } else {
1115  iface->set_visibility_history(1);
1116  }
1117  tf::Vector3 &origin = baserel_pose.getOrigin();
1118  tf::Quaternion quat = baserel_pose.getRotation();
1119  double translation[3] = { origin.x(), origin.y(), origin.z() };
1120  double rotation[4] = { quat.x(), quat.y(), quat.z(), quat.w() };
1121  iface->set_translation(translation);
1122  iface->set_rotation(rotation);
1123 
1124  } else {
1125  if (visibility_history <= 0) {
1126  iface->set_visibility_history(visibility_history - 1);
1127  } else {
1128  iface->set_visibility_history(-1);
1129  double translation[3] = { 0, 0, 0 };
1130  double rotation[4] = { 0, 0, 0, 1 };
1131  iface->set_translation(translation);
1132  iface->set_rotation(rotation);
1133  }
1134  }
1135  iface->write();
1136 }
1137 
1138 
1139 TabletopObjectsThread::CloudPtr
1140 TabletopObjectsThread::generate_table_model(const float length, const float width,
1141  const float thickness, const float step,
1142  const float max_error)
1143 {
1144  CloudPtr c(new Cloud());
1145 
1146  const float length_2 = fabs(length) * 0.5;
1147  const float width_2 = fabs(width) * 0.5;
1148  const float thickness_2 = fabs(thickness) * 0.5;
1149 
1150  // calculate table points
1151  const unsigned int l_base_num = std::max(2u, (unsigned int)floor(length / step));
1152  const unsigned int num_w = l_base_num +
1153  ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1154  const unsigned int w_base_num = std::max(2u, (unsigned int)floor(width / step));
1155  const unsigned int num_h = w_base_num +
1156  ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1157  const unsigned int t_base_num = std::max(2u, (unsigned int)floor(thickness / step));
1158  const unsigned int num_t = t_base_num +
1159  ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1));
1160 
1161  //logger->log_debug(name(), "Generating table model %fx%fx%f (%ux%ux%u=%u points)",
1162  // length, width, thickness,
1163  // num_w, num_h, num_t, num_t * num_w * num_h);
1164 
1165  c->height = 1;
1166  c->width = num_t * num_w * num_h;
1167  c->is_dense = true;
1168  c->points.resize(num_t * num_w * num_h);
1169 
1170  unsigned int idx = 0;
1171  for (unsigned int t = 0; t < num_t; ++t) {
1172  for (unsigned int w = 0; w < num_w; ++w) {
1173  for (unsigned int h = 0; h < num_h; ++h) {
1174  PointType &p = c->points[idx++];
1175 
1176  p.x = h * step - width_2;
1177  if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2;
1178 
1179  p.y = w * step - length_2;
1180  if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2;
1181 
1182  p.z = t * step - thickness_2;
1183  if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error) p.z = thickness_2;
1184  }
1185  }
1186  }
1187 
1188  return c;
1189 }
1190 
1191 TabletopObjectsThread::CloudPtr
1192 TabletopObjectsThread::generate_table_model(const float length, const float width,
1193  const float step, const float max_error)
1194 {
1195  CloudPtr c(new Cloud());
1196 
1197  const float length_2 = fabs(length) * 0.5;
1198  const float width_2 = fabs(width) * 0.5;
1199 
1200  // calculate table points
1201  const unsigned int l_base_num = std::max(2u, (unsigned int)floor(length / step));
1202  const unsigned int num_w = l_base_num +
1203  ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1));
1204  const unsigned int w_base_num = std::max(2u, (unsigned int)floor(width / step));
1205  const unsigned int num_h = w_base_num +
1206  ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1));
1207 
1208  //logger->log_debug(name(), "Generating table model %fx%f (%ux%u=%u points)",
1209  // length, width, num_w, num_h, num_w * num_h);
1210 
1211  c->height = 1;
1212  c->width = num_w * num_h;
1213  c->is_dense = true;
1214  c->points.resize(num_w * num_h);
1215 
1216  unsigned int idx = 0;
1217  for (unsigned int w = 0; w < num_w; ++w) {
1218  for (unsigned int h = 0; h < num_h; ++h) {
1219  PointType &p = c->points[idx++];
1220 
1221  p.x = h * step - width_2;
1222  if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2;
1223 
1224  p.y = w * step - length_2;
1225  if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2;
1226 
1227  p.z = 0.;
1228  }
1229  }
1230 
1231  return c;
1232 }
1233 
1234 
1235 TabletopObjectsThread::CloudPtr
1236 TabletopObjectsThread::simplify_polygon(CloudPtr polygon, float dist_threshold)
1237 {
1238  const float sqr_dist_threshold = dist_threshold * dist_threshold;
1239  CloudPtr result(new Cloud());
1240  const size_t psize = polygon->points.size();
1241  result->points.resize(psize);
1242  size_t res_points = 0;
1243  size_t i_dist = 1;
1244  for (size_t i = 1; i <= psize; ++i) {
1245  PointType &p1p = polygon->points[i - i_dist ];
1246 
1247  if (i == psize) {
1248  if (result->points.empty()) {
1249  // Simplification failed, got something too "line-ish"
1250  return polygon;
1251  }
1252  }
1253 
1254  PointType &p2p = polygon->points[i % psize];
1255  PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize];
1256 
1257  Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0);
1258  Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0);
1259  Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0);
1260 
1261  Eigen::Vector4f line_dir = p3 - p1;
1262 
1263  double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir);
1264  if (sqr_dist < sqr_dist_threshold) {
1265  ++i_dist;
1266  } else {
1267  i_dist = 1;
1268  result->points[res_points++] = p2p;
1269  }
1270  }
1271 
1272  result->header.frame_id = polygon->header.frame_id;
1273  result->header.stamp = polygon->header.stamp;
1274  result->width = res_points;
1275  result->height = 1;
1276  result->is_dense = false;
1277  result->points.resize(res_points);
1278 
1279  return result;
1280 }
1281 
1282 
1283 #ifdef HAVE_VISUAL_DEBUGGING
1284 void
1285 TabletopObjectsThread::set_visualization_thread(TabletopVisualizationThreadBase *visthread)
1286 {
1287  visthread_ = visthread;
1288 }
1289 #endif
void set_frame(const char *new_frame)
Set frame value.
bool is_enabled() const
Get enabled value.
Compare points' distance to a plane.
Definition: comparisons.h:98
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:972
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void remove_pointcloud(const char *id)
Remove the point cloud.
Base class for fawkes tf exceptions.
Definition: exceptions.h:34
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > V_Vector4f
Aligned vector of vectors/points.
A class for handling time.
Definition: time.h:91
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Check if point is inside or outside a given polygon.
Definition: comparisons.h:47
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
Base class for virtualization thread.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
virtual ~TabletopObjectsThread()
Destructor.
Thread aspect to access the transform system.
Definition: tf.h:38
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.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
void reset()
Reset pointer.
Definition: refptr.h:446
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1118
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:105
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.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
Position3DInterface Fawkes BlackBoard Interface.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_enabled(const bool new_enabled)
Set enabled value.
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
Time tracking utility.
Definition: tracker.h:38
DisableSwitchMessage Fawkes BlackBoard Interface Message.
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.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Definition: thread.h:95
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:128
int32_t visibility_history() const
Get visibility_history value.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual void init()
Initialize the thread.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:293
virtual void loop()
Code to execute in the thread.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
tf::TransformListener * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:55
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
virtual void close(Interface *interface)=0
Close interface.
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:54