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> 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> 46 #include <fvcams/shmem.h> 47 #include <fvutils/ipc/shm_image.h> 48 #include <fvutils/adapters/pcl.h> 49 #include <utils/time/time.h> 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 59 template <
typename Po
intT>
60 class PolygonComparison :
public pcl::ComparisonBase<PointT>
62 using pcl::ComparisonBase<PointT>::capable_;
64 typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr;
65 typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr;
68 : inside_(inside), polygon_(polygon)
70 capable_ = (polygon.size() >= 3);
72 virtual ~PolygonComparison() {}
74 virtual bool evaluate(
const PointT &point)
const 77 return pcl::isPointIn2DPolygon(point, polygon_);
79 return ! pcl::isPointIn2DPolygon(point, polygon_);
87 PolygonComparison() {}
91 template <
typename Po
intT>
92 class PlaneDistanceComparison :
public pcl::ComparisonBase<PointT>
94 using pcl::ComparisonBase<PointT>::capable_;
96 typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr;
97 typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr;
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)
104 capable_ = (coeff_->values.size() == 4);
106 virtual ~PlaneDistanceComparison() {}
108 virtual bool evaluate(
const PointT &point)
const 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]);
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_;
127 return val == compare_val_;
132 pcl::ModelCoefficients::ConstPtr coeff_;
133 pcl::ComparisonOps::CompareOp op_;
137 PlaneDistanceComparison() {}
140 template <
typename Po
intType>
141 class OpenNIPlanarSegmentation
145 typedef typename Cloud::Ptr CloudPtr;
146 typedef typename Cloud::ConstPtr CloudConstPtr;
148 OpenNIPlanarSegmentation (
const std::string& device_id =
"",
double threshold = 0.01)
149 : viewer (
"PCL OpenNI Planar Segmentation Viewer"),
150 device_id_ (device_id),
153 grid_.setFilterFieldName (
"z");
154 grid_.setFilterLimits (0.0, 3.0);
155 grid_.setLeafSize (0.01, 0.01, 0.01);
157 seg_.setOptimizeCoefficients (
true);
158 seg_.setModelType (pcl::SACMODEL_PLANE);
159 seg_.setMethodType (pcl::SAC_RANSAC);
160 seg_.setMaxIterations (1000);
161 seg_.setDistanceThreshold (threshold);
166 cloud_cb_ (
const CloudConstPtr& cloud)
172 set (
const CloudConstPtr& cloud)
175 boost::mutex::scoped_lock lock (mtx_);
179 int get_nn(
float x,
float y,
float z)
const 181 PointType p(x, y, z);
182 std::vector<int> indices;
183 std::vector<float> distances;
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);
190 for (
int i = 0; i < count; ++i) {
191 if (distances[i] < min_dist) {
193 min_dist = distances[i];
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];
202 printf(
"No index found looking for (%f, %f, %f)\n", x, y, z);
212 boost::mutex::scoped_lock lock (mtx_);
213 CloudPtr temp_cloud (
new Cloud);
214 CloudPtr temp_cloud2 (
new Cloud);
216 grid_.setInputCloud (cloud_);
217 grid_.filter (*temp_cloud);
221 for (p = temp_cloud->begin(); p != temp_cloud->end(); ++p) {
227 pcl::ModelCoefficients::Ptr coefficients (
new pcl::ModelCoefficients ());
228 pcl::PointIndices::Ptr inliers (
new pcl::PointIndices ());
230 seg_.setInputCloud (temp_cloud);
231 seg_.segment (*inliers, *coefficients);
233 extract_.setNegative (
false);
234 extract_.setInputCloud (temp_cloud);
235 extract_.setIndices (inliers);
236 extract_.filter (*temp_cloud2);
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_);
250 pcl::ConvexHull<PointType> hr;
252 hr.setInputCloud(cloud_proj_);
253 cloud_hull_.reset(
new Cloud());
254 hr.reconstruct (*cloud_hull_, vertices_);
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;
266 cloud_filt_.reset(
new Cloud());
267 extract_.setNegative(
true);
268 extract_.filter(*cloud_filt_);
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_);
279 cloud_above_.reset(
new Cloud());
280 above_condrem.filter(*cloud_above_);
282 printf(
"Before: %zu After: %zu\n", cloud_filt_->points.size(),
283 cloud_above_->points.size());
286 if (! vertices_.empty()) {
287 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
288 polygon->indices = vertices_[0].vertices;
291 pcl::ExtractIndices<PointType> polygon_extract;
293 polygon_extract.setInputCloud(cloud_hull_);
294 polygon_extract.setIndices(polygon);
295 polygon_extract.filter(polygon_cloud);
297 typename pcl::ConditionAnd<PointType>::Ptr
298 polygon_cond(
new pcl::ConditionAnd<PointType>());
300 typename PolygonComparison<PointType>::ConstPtr
301 inpoly_comp(
new PolygonComparison<PointType>(polygon_cloud));
302 polygon_cond->addComparison(inpoly_comp);
305 pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
306 condrem.setInputCloud(cloud_above_);
308 cloud_objs_.reset(
new Cloud());
309 condrem.filter(*cloud_objs_);
311 cloud_objs_.reset(
new Cloud(*cloud_above_));
318 pcl::KdTree<pcl::PointXYZRGB>::Ptr
319 kdtree_cl(
new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
320 kdtree_cl->setInputCloud(cloud_objs_);
322 std::vector<pcl::PointIndices> cluster_indices;
323 pcl::EuclideanClusterExtraction<PointType> ec;
324 ec.setClusterTolerance(0.04);
325 ec.setMinClusterSize(50);
326 ec.setMaxClusterSize(25000);
327 ec.setSearchMethod(kdtree_cl);
328 ec.setInputCloud(cloud_objs_);
329 ec.extract(cluster_indices);
331 printf(
"Found %zu clusters\n", cluster_indices.size());
333 uint8_t colors[5][3] = { {255, 0, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255},
336 std::vector<pcl::PointIndices>::const_iterator it;
337 unsigned int color = 0;
339 for (it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {
342 r = colors[color][0];
343 g = colors[color][1];
344 b = colors[color][2];
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);
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;
441 return (cloud_above_);
445 viz_cb (pcl::visualization::PCLVisualizer& viz)
449 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
454 boost::mutex::scoped_lock lock (mtx_);
456 if (!viz.updatePointCloud(cloud_proj_,
"table")) {
457 viz.addPointCloud (cloud_proj_,
"table");
459 if (!viz.updatePointCloud(cloud_objs_,
"clusters")) {
460 viz.addPointCloud (cloud_objs_,
"clusters");
463 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4,
"table");
464 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4,
"clusters");
466 viz.removeShape (
"hull");
467 if (!vertices_.empty()) {
468 viz.addPolygonMesh<PointType>(cloud_hull_, vertices_,
"hull");
485 viewer.runOnVisualizationThread(boost::bind(&OpenNIPlanarSegmentation::viz_cb,
486 this, _1),
"viz_cb");
494 while (!viewer.wasStopped ())
497 if (last_update != ct) {
502 CloudPtr cloud(
new Cloud());
503 convert_buffer_to_pcl(buf, *cloud);
507 viewer.showCloud(
get());
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_;
526 std::string device_id_;
528 CloudConstPtr cloud_;
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";
538 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
539 if (driver.getNumberDevices () > 0)
541 for (
unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
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;
551 cout <<
"No devices connected." << endl;
555 main (
int argc,
char ** argv)
563 std::string arg (argv[1]);
565 if (arg ==
"--help" || arg ==
"-h")
571 double threshold = 0.05;
572 pcl::console::parse_argument (argc, argv,
"-thresh", threshold);
577 OpenNIPlanarSegmentation<pcl::PointXYZRGB> v (arg);
fawkes::Time capture_time() const
Get the time when the image was captured.
Fawkes library namespace.
A class for handling time.
virtual void capture()=0
Capture an image.
Shared memory image buffer.