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
58using namespace fawkes;
59using namespace firevision;
60
61template <typename PointT>
62class PolygonComparison : public pcl::ComparisonBase<PointT>
63{
64 using pcl::ComparisonBase<PointT>::capable_;
65
66public:
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
88protected:
89 bool inside_;
90 const pcl::PointCloud<PointT> &polygon_;
91
92private:
93 PolygonComparison()
94 {
95 } // not allowed
96};
97
98template <typename PointT>
99class PlaneDistanceComparison : public pcl::ComparisonBase<PointT>
100{
101 using pcl::ComparisonBase<PointT>::capable_;
102
103public:
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
142protected:
143 pcl::ModelCoefficients::ConstPtr coeff_;
144 pcl::ComparisonOps::CompareOp op_;
145 float compare_val_;
146
147private:
148 PlaneDistanceComparison()
149 {
150 } // not allowed
151};
152
153template <typename PointType>
154class OpenNIPlanarSegmentation
155{
156public:
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
539void
540usage(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
566int
567main(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
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.