24 #include <pcl/point_cloud.h> 25 #include <pcl/point_types.h> 27 #include <boost/date_time/posix_time/posix_time.hpp> 28 #include <boost/thread/thread.hpp> 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> 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 59 using namespace firevision;
61 template <
typename Po
intT>
62 class PolygonComparison :
public pcl::ComparisonBase<PointT>
64 using pcl::ComparisonBase<PointT>::capable_;
67 typedef boost::shared_ptr<PolygonComparison<PointT>> Ptr;
68 typedef boost::shared_ptr<const PolygonComparison<PointT>> ConstPtr;
71 : inside_(inside), polygon_(polygon)
73 capable_ = (polygon.size() >= 3);
75 virtual ~PolygonComparison()
80 evaluate(
const PointT &point)
const 83 return pcl::isPointIn2DPolygon(point, polygon_);
85 return !pcl::isPointIn2DPolygon(point, polygon_);
98 template <
typename Po
intT>
99 class PlaneDistanceComparison :
public pcl::ComparisonBase<PointT>
101 using pcl::ComparisonBase<PointT>::capable_;
104 typedef boost::shared_ptr<PlaneDistanceComparison<PointT>> Ptr;
105 typedef boost::shared_ptr<const PlaneDistanceComparison<PointT>> ConstPtr;
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)
112 capable_ = (coeff_->values.size() == 4);
114 virtual ~PlaneDistanceComparison()
119 evaluate(
const PointT &point)
const 122 (coeff_->values[0] * point.x + coeff_->values[1] * point.y + coeff_->values[2] * point.z
124 / sqrtf(coeff_->values[0] * coeff_->values[0] + coeff_->values[1] * coeff_->values[1]
125 + coeff_->values[2] * coeff_->values[2]);
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_;
138 return val == compare_val_;
143 pcl::ModelCoefficients::ConstPtr coeff_;
144 pcl::ComparisonOps::CompareOp op_;
148 PlaneDistanceComparison()
153 template <
typename Po
intType>
154 class OpenNIPlanarSegmentation
158 typedef typename Cloud::Ptr CloudPtr;
159 typedef typename Cloud::ConstPtr CloudConstPtr;
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)
164 grid_.setFilterFieldName(
"z");
165 grid_.setFilterLimits(0.0, 3.0);
166 grid_.setLeafSize(0.01, 0.01, 0.01);
168 seg_.setOptimizeCoefficients(
true);
169 seg_.setModelType(pcl::SACMODEL_PLANE);
170 seg_.setMethodType(pcl::SAC_RANSAC);
171 seg_.setMaxIterations(1000);
172 seg_.setDistanceThreshold(threshold);
176 cloud_cb_(
const CloudConstPtr &cloud)
182 set(
const CloudConstPtr &cloud)
185 boost::mutex::scoped_lock lock(mtx_);
190 get_nn(
float x,
float y,
float z)
const 192 PointType p(x, y, z);
193 std::vector<int> indices;
194 std::vector<float> distances;
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);
201 for (
int i = 0; i < count; ++i) {
202 if (distances[i] < min_dist) {
204 min_dist = distances[i];
207 printf(
"Found at dist %f (%f, %f, %f)\n",
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];
214 printf(
"No index found looking for (%f, %f, %f)\n", x, y, z);
224 boost::mutex::scoped_lock lock(mtx_);
225 CloudPtr temp_cloud(
new Cloud);
226 CloudPtr temp_cloud2(
new Cloud);
228 grid_.setInputCloud(cloud_);
229 grid_.filter(*temp_cloud);
233 for (p = temp_cloud->begin(); p != temp_cloud->end(); ++p) {
239 pcl::ModelCoefficients::Ptr coefficients(
new pcl::ModelCoefficients());
240 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
242 seg_.setInputCloud(temp_cloud);
243 seg_.segment(*inliers, *coefficients);
245 extract_.setNegative(
false);
246 extract_.setInputCloud(temp_cloud);
247 extract_.setIndices(inliers);
248 extract_.filter(*temp_cloud2);
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_);
261 pcl::ConvexHull<PointType> hr;
263 hr.setInputCloud(cloud_proj_);
264 cloud_hull_.reset(
new Cloud());
265 hr.reconstruct(*cloud_hull_, vertices_);
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;
277 cloud_filt_.reset(
new Cloud());
278 extract_.setNegative(
true);
279 extract_.filter(*cloud_filt_);
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_);
289 cloud_above_.reset(
new Cloud());
290 above_condrem.filter(*cloud_above_);
292 printf(
"Before: %zu After: %zu\n", cloud_filt_->points.size(), cloud_above_->points.size());
295 if (!vertices_.empty()) {
296 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
297 polygon->indices = vertices_[0].vertices;
300 pcl::ExtractIndices<PointType> polygon_extract;
302 polygon_extract.setInputCloud(cloud_hull_);
303 polygon_extract.setIndices(polygon);
304 polygon_extract.filter(polygon_cloud);
306 typename pcl::ConditionAnd<PointType>::Ptr polygon_cond(
new pcl::ConditionAnd<PointType>());
308 typename PolygonComparison<PointType>::ConstPtr inpoly_comp(
309 new PolygonComparison<PointType>(polygon_cloud));
310 polygon_cond->addComparison(inpoly_comp);
313 pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
314 condrem.setInputCloud(cloud_above_);
316 cloud_objs_.reset(
new Cloud());
317 condrem.filter(*cloud_objs_);
319 cloud_objs_.reset(
new Cloud(*cloud_above_));
326 pcl::KdTree<pcl::PointXYZRGB>::Ptr kdtree_cl(
new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
327 kdtree_cl->setInputCloud(cloud_objs_);
329 std::vector<pcl::PointIndices> cluster_indices;
330 pcl::EuclideanClusterExtraction<PointType> ec;
331 ec.setClusterTolerance(0.04);
332 ec.setMinClusterSize(50);
333 ec.setMaxClusterSize(25000);
334 ec.setSearchMethod(kdtree_cl);
335 ec.setInputCloud(cloud_objs_);
336 ec.extract(cluster_indices);
338 printf(
"Found %zu clusters\n", cluster_indices.size());
340 uint8_t colors[5][3] = {{255, 0, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255}, {0, 255, 255}};
342 std::vector<pcl::PointIndices>::const_iterator it;
343 unsigned int color = 0;
345 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
348 r = colors[color][0];
349 g = colors[color][1];
350 b = colors[color][2];
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);
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;
446 return (cloud_above_);
450 viz_cb(pcl::visualization::PCLVisualizer &viz)
453 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
458 boost::mutex::scoped_lock lock(mtx_);
460 if (!viz.updatePointCloud(cloud_proj_,
"table")) {
461 viz.addPointCloud(cloud_proj_,
"table");
463 if (!viz.updatePointCloud(cloud_objs_,
"clusters")) {
464 viz.addPointCloud(cloud_objs_,
"clusters");
467 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
470 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
474 viz.removeShape(
"hull");
475 if (!vertices_.empty()) {
476 viz.addPolygonMesh<PointType>(cloud_hull_, vertices_,
"hull");
493 viewer.runOnVisualizationThread(boost::bind(&OpenNIPlanarSegmentation::viz_cb,
this, _1),
502 while (!viewer.wasStopped()) {
504 if (last_update != ct) {
509 CloudPtr cloud(
new Cloud());
510 convert_buffer_to_pcl(buf, *cloud);
514 viewer.showCloud(get());
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_;
533 std::string device_id_;
535 CloudConstPtr cloud_;
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";
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" 558 <<
" <serial-number> (only in Linux and for devices which provide " 563 cout <<
"No devices connected." << endl;
567 main(
int argc,
char **argv)
574 std::string arg(argv[1]);
576 if (arg ==
"--help" || arg ==
"-h") {
581 double threshold = 0.05;
582 pcl::console::parse_argument(argc, argv,
"-thresh", threshold);
587 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.