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