Fawkes API  Fawkes Development Version
tabletop_objects_standalone.cpp
1 
2 /***************************************************************************
3  * tabletop_objects_standalone.cpp - Thread to detect tabletop objects
4  *
5  * Created: Sat Oct 01 11:51:00 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 /// @cond EXAMPLE
23 
24 #include <boost/thread/thread.hpp>
25 #include <boost/date_time/posix_time/posix_time.hpp>
26 #include <pcl/point_cloud.h>
27 #include <pcl/point_types.h>
28 // include <pcl/io/openni_grabber.h>
29 #include <pcl/visualization/cloud_viewer.h>
30 #include <pcl/io/openni_camera/openni_driver.h>
31 #include <pcl/console/parse.h>
32 #include <pcl/sample_consensus/method_types.h>
33 #include <pcl/sample_consensus/model_types.h>
34 #include <pcl/segmentation/sac_segmentation.h>
35 #include <pcl/segmentation/extract_clusters.h>
36 #include <pcl/filters/voxel_grid.h>
37 #include <pcl/filters/extract_indices.h>
38 #include <pcl/surface/concave_hull.h>
39 #include <pcl/surface/convex_hull.h>
40 #include <pcl/kdtree/kdtree.h>
41 #include <pcl/kdtree/kdtree_flann.h>
42 #include <pcl/filters/project_inliers.h>
43 #include <pcl/filters/conditional_removal.h>
44 #include <pcl/segmentation/extract_polygonal_prism_data.h>
45 
46 #include <fvcams/shmem.h>
47 #include <fvutils/ipc/shm_image.h>
48 #include <fvutils/adapters/pcl.h>
49 #include <utils/time/time.h>
50 
51 #define TABLE_MAX_X 3.0
52 #define TABLE_MAX_Y 3.0
53 #define TABLE_MIN_X -3.0
54 #define TABLE_MIN_Y -3.0
55 
56 using namespace fawkes;
57 using namespace firevision;
58 
59 template <typename PointT>
60 class PolygonComparison : public pcl::ComparisonBase<PointT>
61 {
62  using pcl::ComparisonBase<PointT>::capable_;
63  public:
64  typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr;
65  typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr;
66 
67  PolygonComparison(const pcl::PointCloud<PointT> &polygon, bool inside = true)
68  : inside_(inside), polygon_(polygon)
69  {
70  capable_ = (polygon.size() >= 3);
71  }
72  virtual ~PolygonComparison() {}
73 
74  virtual bool evaluate(const PointT &point) const
75  {
76  if (inside_)
77  return pcl::isPointIn2DPolygon(point, polygon_);
78  else
79  return ! pcl::isPointIn2DPolygon(point, polygon_);
80  }
81 
82  protected:
83  bool inside_;
84  const pcl::PointCloud<PointT> &polygon_;
85 
86  private:
87  PolygonComparison() {} // not allowed
88 };
89 
90 
91 template <typename PointT>
92 class PlaneDistanceComparison : public pcl::ComparisonBase<PointT>
93 {
94  using pcl::ComparisonBase<PointT>::capable_;
95  public:
96  typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr;
97  typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr;
98 
99  PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff,
100  pcl::ComparisonOps::CompareOp op = pcl::ComparisonOps::GT,
101  float compare_val = 0.)
102  : coeff_(coeff), op_(op), compare_val_(compare_val)
103  {
104  capable_ = (coeff_->values.size() == 4);
105  }
106  virtual ~PlaneDistanceComparison() {}
107 
108  virtual bool evaluate(const PointT &point) const
109  {
110  float val = (coeff_->values[0] * point.x + coeff_->values[1] * point.y +
111  coeff_->values[2] * point.z + coeff_->values[3]) /
112  sqrtf(coeff_->values[0] * coeff_->values[0] +
113  coeff_->values[1] * coeff_->values[1] +
114  coeff_->values[2] * coeff_->values[2]);
115 
116  //printf("%f > %f?: %d\n", val, compare_val_, (val > compare_val_));
117 
118  if (op_ == pcl::ComparisonOps::GT) {
119  return val > compare_val_;
120  } else if (op_ == pcl::ComparisonOps::GE) {
121  return val >= compare_val_;
122  } else if (op_ == pcl::ComparisonOps::LT) {
123  return val < compare_val_;
124  } else if (op_ == pcl::ComparisonOps::LE) {
125  return val <= compare_val_;
126  } else {
127  return val == compare_val_;
128  }
129  }
130 
131  protected:
132  pcl::ModelCoefficients::ConstPtr coeff_;
133  pcl::ComparisonOps::CompareOp op_;
134  float compare_val_;
135 
136  private:
137  PlaneDistanceComparison() {} // not allowed
138 };
139 
140 template <typename PointType>
141 class OpenNIPlanarSegmentation
142 {
143 public:
144  typedef pcl::PointCloud<PointType> Cloud;
145  typedef typename Cloud::Ptr CloudPtr;
146  typedef typename Cloud::ConstPtr CloudConstPtr;
147 
148  OpenNIPlanarSegmentation (const std::string& device_id = "", double threshold = 0.01)
149  : viewer ("PCL OpenNI Planar Segmentation Viewer"),
150  device_id_ (device_id),
151  new_cloud_(false)
152  {
153  grid_.setFilterFieldName ("z");
154  grid_.setFilterLimits (0.0, 3.0);
155  grid_.setLeafSize (0.01, 0.01, 0.01);
156 
157  seg_.setOptimizeCoefficients (true);
158  seg_.setModelType (pcl::SACMODEL_PLANE);
159  seg_.setMethodType (pcl::SAC_RANSAC);
160  seg_.setMaxIterations (1000);
161  seg_.setDistanceThreshold (threshold);
162 
163  }
164 
165  void
166  cloud_cb_ (const CloudConstPtr& cloud)
167  {
168  set (cloud);
169  }
170 
171  void
172  set (const CloudConstPtr& cloud)
173  {
174  //lock while we set our cloud;
175  boost::mutex::scoped_lock lock (mtx_);
176  cloud_ = cloud;
177  }
178 
179  int get_nn(float x, float y, float z) const
180  {
181  PointType p(x, y, z);
182  std::vector<int> indices;
183  std::vector<float> distances;
184 
185  float min_dist = HUGE;
186  int count = kdtree_.radiusSearch(p, 1.0, indices, distances);
187  if (! indices.empty()) {
188  printf("Got %i indices!\n", count);
189  int index = 0;
190  for (int i = 0; i < count; ++i) {
191  if (distances[i] < min_dist) {
192  index = i;
193  min_dist = distances[i];
194  }
195  }
196  printf("Found at dist %f (%f, %f, %f)\n", distances[index],
197  cloud_proj_->points[indices[index]].x,
198  cloud_proj_->points[indices[index]].y,
199  cloud_proj_->points[indices[index]].z);
200  return indices[index];
201  } else {
202  printf("No index found looking for (%f, %f, %f)\n", x, y, z);
203  }
204 
205  return -1;
206  }
207 
208  CloudPtr
209  get ()
210  {
211  //lock while we swap our cloud and reset it.
212  boost::mutex::scoped_lock lock (mtx_);
213  CloudPtr temp_cloud (new Cloud);
214  CloudPtr temp_cloud2 (new Cloud);
215 
216  grid_.setInputCloud (cloud_);
217  grid_.filter (*temp_cloud);
218 
219  // set all colors to white for better distinguishing the pixels
221  for (p = temp_cloud->begin(); p != temp_cloud->end(); ++p) {
222  p->r = 255;
223  p->g = 255;
224  p->b = 255;
225  }
226 
227  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
228  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
229 
230  seg_.setInputCloud (temp_cloud);
231  seg_.segment (*inliers, *coefficients);
232 
233  extract_.setNegative (false);
234  extract_.setInputCloud (temp_cloud);
235  extract_.setIndices (inliers);
236  extract_.filter (*temp_cloud2);
237 
238  // Project the model inliers
239  pcl::ProjectInliers<PointType> proj;
240  proj.setModelType(pcl::SACMODEL_PLANE);
241  proj.setInputCloud(temp_cloud2);
242  proj.setModelCoefficients(coefficients);
243  cloud_proj_.reset(new Cloud());
244  proj.filter (*cloud_proj_);
245  //printf("PointCloud after projection has: %zu data points.\n",
246  // cloud_proj_->points.size());
247 
248 
249  // Estimate 3D convex hull -> TABLE BOUNDARIES
250  pcl::ConvexHull<PointType> hr;
251  //hr.setAlpha (0.1); // only for ConcaveHull
252  hr.setInputCloud(cloud_proj_);
253  cloud_hull_.reset(new Cloud());
254  hr.reconstruct (*cloud_hull_, vertices_);
255 
256  //printf("Found %zu vertices, first has size %zu\n",
257  // vertices_.size(), vertices_[0].vertices.size());
258 
259  for (size_t i = 0; i < cloud_proj_->points.size(); ++i) {
260  cloud_proj_->points[i].r = 0;
261  cloud_proj_->points[i].g = 255;
262  cloud_proj_->points[i].b = 0;
263  }
264 
265  // Extrat all non-plane points
266  cloud_filt_.reset(new Cloud());
267  extract_.setNegative(true);
268  extract_.filter(*cloud_filt_);
269 
270  // remove all pixels below table
271  typename PlaneDistanceComparison<PointType>::ConstPtr
272  above_comp(new PlaneDistanceComparison<PointType>(coefficients, pcl::ComparisonOps::LT));
273  typename pcl::ConditionAnd<PointType>::Ptr
274  above_cond(new pcl::ConditionAnd<PointType>());
275  above_cond->addComparison(above_comp);
276  pcl::ConditionalRemoval<PointType> above_condrem(above_cond);
277  above_condrem.setInputCloud(cloud_filt_);
278  //above_condrem.setKeepOrganized(true);
279  cloud_above_.reset(new Cloud());
280  above_condrem.filter(*cloud_above_);
281 
282  printf("Before: %zu After: %zu\n", cloud_filt_->points.size(),
283  cloud_above_->points.size());
284 
285  // Extract only points on the table plane
286  if (! vertices_.empty()) {
287  pcl::PointIndices::Ptr polygon(new pcl::PointIndices());
288  polygon->indices = vertices_[0].vertices;
289 
290  pcl::PointCloud<PointType> polygon_cloud;
291  pcl::ExtractIndices<PointType> polygon_extract;
292 
293  polygon_extract.setInputCloud(cloud_hull_);
294  polygon_extract.setIndices(polygon);
295  polygon_extract.filter(polygon_cloud);
296 
297  typename pcl::ConditionAnd<PointType>::Ptr
298  polygon_cond(new pcl::ConditionAnd<PointType>());
299 
300  typename PolygonComparison<PointType>::ConstPtr
301  inpoly_comp(new PolygonComparison<PointType>(polygon_cloud));
302  polygon_cond->addComparison(inpoly_comp);
303 
304  // build the filter
305  pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
306  condrem.setInputCloud(cloud_above_);
307  //condrem.setKeepOrganized(true);
308  cloud_objs_.reset(new Cloud());
309  condrem.filter(*cloud_objs_);
310  } else {
311  cloud_objs_.reset(new Cloud(*cloud_above_));
312  }
313 
314  // CLUSTERS
315  // extract clusters of OBJECTS
316 
317  // Creating the KdTree object for the search method of the extraction
318  pcl::KdTree<pcl::PointXYZRGB>::Ptr
319  kdtree_cl(new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
320  kdtree_cl->setInputCloud(cloud_objs_);
321 
322  std::vector<pcl::PointIndices> cluster_indices;
323  pcl::EuclideanClusterExtraction<PointType> ec;
324  ec.setClusterTolerance(0.04); // 2cm
325  ec.setMinClusterSize(50);
326  ec.setMaxClusterSize(25000);
327  ec.setSearchMethod(kdtree_cl);
328  ec.setInputCloud(cloud_objs_);
329  ec.extract(cluster_indices);
330 
331  printf("Found %zu clusters\n", cluster_indices.size());
332 
333  uint8_t colors[5][3] = { {255, 0, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255},
334  {0, 255, 255} };
335 
336  std::vector<pcl::PointIndices>::const_iterator it;
337  unsigned int color = 0;
338  unsigned int i = 0;
339  for (it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {
340  uint8_t r, g, b;
341  if (color < 5) {
342  r = colors[color][0];
343  g = colors[color][1];
344  b = colors[color][2];
345  ++color;
346  } else {
347  double dr=0, dg=0, db=0;
348  pcl::visualization::getRandomColors(dr, dg, db);
349  r = (uint8_t)roundf(dr * 255);
350  g = (uint8_t)roundf(dg * 255);
351  b = (uint8_t)roundf(db * 255);
352  }
353  printf("Cluster %u size: %zu color %u, %u, %u\n",
354  ++i, it->indices.size(), r, g, b);
355  std::vector<int>::const_iterator pit;
356  for (pit = it->indices.begin (); pit != it->indices.end(); pit++) {
357  cloud_objs_->points[*pit].r = r;
358  cloud_objs_->points[*pit].g = g;
359  cloud_objs_->points[*pit].b = b;
360  }
361  }
362 
363  /* To project into Z:
364  // Create a set of planar coefficients with X=Y=0,Z=1
365  pcl::ModelCoefficients::Ptr proj_coeff (new pcl::ModelCoefficients ());
366  proj_coeff->values.resize (4);
367  proj_coeff->values[0] = proj_coeff->values[1] = 0;
368  proj_coeff->values[2] = 1.0;
369  proj_coeff->values[3] = 0;
370 
371  // Create the filtering object
372  pcl::ProjectInliers<PointType> proj;
373  proj.setModelType(pcl::SACMODEL_PLANE);
374  proj.setInputCloud(cloud_hull_);
375  proj.setModelCoefficients(proj_coeff);
376  cloud_proj_.reset(new Cloud());
377  proj.filter(*cloud_proj_);
378 
379  printf("Vertices: %zu, first: %zu\n", vertices_.size(), vertices_[0].vertices.size());
380  vertices_.resize(5);
381 
382  //get the extents of the table
383  if (! cloud_proj_->points.empty()) {
384  printf("Cloud hull non-empty\n");
385  float x_min = HUGE, y_min = HUGE, x_max = 0, y_max = 0;
386 
387  for (size_t i = 1; i < cloud_proj_->points.size(); ++i) {
388  if (cloud_proj_->points[i].x < x_min && cloud_proj_->points[i].x > -3.0)
389  x_min = cloud_proj_->points[i].x;
390  if (cloud_proj_->points[i].x > x_max && cloud_proj_->points[i].x < 3.0)
391  x_max = cloud_proj_->points[i].x;
392  if (cloud_proj_->points[i].y < y_min && cloud_proj_->points[i].y > -3.0)
393  y_min = cloud_proj_->points[i].y;
394  if (cloud_proj_->points[i].y > y_max && cloud_proj_->points[i].y < 3.0)
395  y_max = cloud_proj_->points[i].y;
396  }
397 
398  kdtree_.setInputCloud(cloud_proj_);
399  float table_z = cloud_proj_->points[0].z; // only need a very rough estimate
400 
401  printf("x_min=%f x_max=%f y_min=%f y_max=%f table_z = %f\n",
402  x_min, x_max, y_min, y_max, table_z);
403 
404  int blp = get_nn(x_min, y_min, table_z);
405  int brp = get_nn(x_max, y_min, table_z);
406  int tlp = get_nn(x_min, y_max, table_z);
407  int trp = get_nn(x_max, y_max, table_z);
408 
409  printf("blp=%i (%f,%f,%f) brp=%i (%f,%f,%f) "
410  "tlp=%i (%f,%f,%f) trp=%i (%f,%f,%f)\n",
411  blp, cloud_proj_->points[blp].x,
412  cloud_proj_->points[blp].y, cloud_proj_->points[blp].z,
413  brp, cloud_proj_->points[brp].x,
414  cloud_proj_->points[brp].y, cloud_proj_->points[brp].z,
415  tlp, cloud_proj_->points[tlp].x,
416  cloud_proj_->points[tlp].y, cloud_proj_->points[tlp].z,
417  trp, cloud_proj_->points[trp].x,
418  cloud_proj_->points[trp].y, cloud_proj_->points[trp].z);
419 
420  pcl::Vertices v;
421  v.vertices.push_back(blp);
422  v.vertices.push_back(tlp);
423  v.vertices.push_back(trp);
424  v.vertices.push_back(brp);
425  v.vertices.push_back(blp);
426  vertices_.clear();
427  vertices_.push_back(v);
428  }
429 */
430 
431  new_cloud_ = true;
432 
433  // To show differences between cloud_filt and cloud_above
434  // (draw both, increase point size of cloud_above
435  //for (int i = 0; i < cloud_filt_->points.size(); ++i) {
436  // cloud_filt_->points[i].r = 255;
437  // cloud_filt_->points[i].g = 0;
438  // cloud_filt_->points[i].b = 0;
439  //}
440 
441  return (cloud_above_);
442  }
443 
444  void
445  viz_cb (pcl::visualization::PCLVisualizer& viz)
446  {
447  if (!new_cloud_)
448  {
449  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
450  return;
451  }
452 
453  {
454  boost::mutex::scoped_lock lock (mtx_);
455  // Render the data
456  if (!viz.updatePointCloud(cloud_proj_, "table")) {
457  viz.addPointCloud (cloud_proj_, "table");
458  }
459  if (!viz.updatePointCloud(cloud_objs_, "clusters")) {
460  viz.addPointCloud (cloud_objs_, "clusters");
461  }
462 
463  viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "table");
464  viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "clusters");
465 
466  viz.removeShape ("hull");
467  if (!vertices_.empty()) {
468  viz.addPolygonMesh<PointType>(cloud_hull_, vertices_, "hull");
469  }
470  new_cloud_ = false;
471  }
472  }
473 
474  void
475  run ()
476  {
477  /*
478  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id_);
479 
480  boost::function<void (const CloudConstPtr&)> f =
481  boost::bind (&OpenNIPlanarSegmentation::cloud_cb_, this, _1);
482  boost::signals2::connection c = interface->registerCallback(f);
483  */
484 
485  viewer.runOnVisualizationThread(boost::bind(&OpenNIPlanarSegmentation::viz_cb,
486  this, _1), "viz_cb");
487 
488  SharedMemoryCamera cam("openni-pointcloud");
489  SharedMemoryImageBuffer *buf = cam.shared_memory_image_buffer();
490  //interface->start ();
491 
492  fawkes::Time last_update;
493 
494  while (!viewer.wasStopped ())
495  {
496  fawkes::Time ct = buf->capture_time();
497  if (last_update != ct) {
498  last_update = ct;
499 
500  cam.capture();
501 
502  CloudPtr cloud(new Cloud());
503  convert_buffer_to_pcl(buf, *cloud);
504  set(cloud);
505 
506  //the call to get() sets the cloud_ to null;
507  viewer.showCloud(get());
508  }
509  }
510 
511  //interface->stop ();
512  }
513 
514  pcl::visualization::CloudViewer viewer;
515  pcl::VoxelGrid<PointType> grid_;
516  pcl::SACSegmentation<PointType> seg_;
517  pcl::ExtractIndices<PointType> extract_;
518  std::vector<pcl::Vertices> vertices_;
519  CloudPtr cloud_hull_;
520  CloudPtr cloud_proj_;
521  CloudPtr cloud_filt_;
522  CloudPtr cloud_above_;
523  CloudPtr cloud_objs_;
524  pcl::KdTreeFLANN<PointType> kdtree_;
525 
526  std::string device_id_;
527  boost::mutex mtx_;
528  CloudConstPtr cloud_;
529  bool new_cloud_;
530 };
531 
532 void
533 usage (char ** argv)
534 {
535  std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
536  << "where options are:\n -thresh X :: set the planar segmentation threshold (default: 0.5)\n";
537 
538  openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
539  if (driver.getNumberDevices () > 0)
540  {
541  for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
542  {
543  cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
544  << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
545  cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl
546  << " bus@address for the device connected to a specific usb-bus / address combination (wotks only in Linux) or" << endl
547  << " <serial-number> (only in Linux and for devices which provide serial numbers)" << endl;
548  }
549  }
550  else
551  cout << "No devices connected." << endl;
552 }
553 
554 int
555 main (int argc, char ** argv)
556 {
557  if (argc < 2)
558  {
559  usage (argv);
560  return 1;
561  }
562 
563  std::string arg (argv[1]);
564 
565  if (arg == "--help" || arg == "-h")
566  {
567  usage (argv);
568  return 1;
569  }
570 
571  double threshold = 0.05;
572  pcl::console::parse_argument (argc, argv, "-thresh", threshold);
573 
574  //pcl::OpenNIGrabber grabber (arg);
575  //if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
576  //{
577  OpenNIPlanarSegmentation<pcl::PointXYZRGB> v (arg);
578  v.run ();
579  /*
580  }
581  else
582  {
583  printf("Only RGBD supported atm\n");
584  //OpenNIPlanarSegmentation<pcl::PointXYZ> v (arg, threshold);
585  //v.run ();
586  }
587  */
588 
589  return (0);
590 }
591 
592 /// @endcond EXAMPLE
fawkes::Time capture_time() const
Get the time when the image was captured.
Definition: shm_image.cpp:195
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
virtual void capture()=0
Capture an image.
Shared memory image buffer.
Definition: shm_image.h:181
Shared memory camera.
Definition: shmem.h:38