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