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> 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 61 template <
typename Po
intT>
62 class PolygonComparison :
public pcl::ComparisonBase<PointT>
64 using pcl::ComparisonBase<PointT>::capable_;
66 typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr;
67 typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr;
70 : inside_(inside), polygon_(polygon)
72 capable_ = (polygon.size() >= 3);
74 virtual ~PolygonComparison() {}
76 virtual bool evaluate(
const PointT &point)
const 79 return pcl::isPointIn2DPolygon(point, polygon_);
81 return ! pcl::isPointIn2DPolygon(point, polygon_);
89 PolygonComparison() {}
93 template <
typename Po
intT>
94 class PlaneDistanceComparison :
public pcl::ComparisonBase<PointT>
96 using pcl::ComparisonBase<PointT>::capable_;
98 typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr;
99 typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr;
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)
106 capable_ = (coeff_->values.size() == 4);
108 virtual ~PlaneDistanceComparison() {}
110 virtual bool evaluate(
const PointT &point)
const 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]);
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_;
129 return val == compare_val_;
134 pcl::ModelCoefficients::ConstPtr coeff_;
135 pcl::ComparisonOps::CompareOp op_;
139 PlaneDistanceComparison() {}
142 template <
typename Po
intType>
143 class OpenNIPlanarSegmentation
147 typedef typename Cloud::Ptr CloudPtr;
148 typedef typename Cloud::ConstPtr CloudConstPtr;
150 OpenNIPlanarSegmentation (
const std::string& device_id =
"",
double threshold = 0.01)
151 : viewer (
"PCL OpenNI Planar Segmentation Viewer"),
152 device_id_ (device_id),
155 grid_.setFilterFieldName (
"z");
156 grid_.setFilterLimits (0.0, 3.0);
157 grid_.setLeafSize (0.01, 0.01, 0.01);
159 seg_.setOptimizeCoefficients (
true);
160 seg_.setModelType (pcl::SACMODEL_PLANE);
161 seg_.setMethodType (pcl::SAC_RANSAC);
162 seg_.setMaxIterations (1000);
163 seg_.setDistanceThreshold (threshold);
168 cloud_cb_ (
const CloudConstPtr& cloud)
174 set (
const CloudConstPtr& cloud)
177 boost::mutex::scoped_lock lock (mtx_);
181 int get_nn(
float x,
float y,
float z)
const 183 PointType p(x, y, z);
184 std::vector<int> indices;
185 std::vector<float> distances;
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);
192 for (
int i = 0; i < count; ++i) {
193 if (distances[i] < min_dist) {
195 min_dist = distances[i];
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];
204 printf(
"No index found looking for (%f, %f, %f)\n", x, y, z);
214 boost::mutex::scoped_lock lock (mtx_);
215 CloudPtr temp_cloud (
new Cloud);
216 CloudPtr temp_cloud2 (
new Cloud);
218 grid_.setInputCloud (cloud_);
219 grid_.filter (*temp_cloud);
223 for (p = temp_cloud->begin(); p != temp_cloud->end(); ++p) {
229 pcl::ModelCoefficients::Ptr coefficients (
new pcl::ModelCoefficients ());
230 pcl::PointIndices::Ptr inliers (
new pcl::PointIndices ());
232 seg_.setInputCloud (temp_cloud);
233 seg_.segment (*inliers, *coefficients);
235 extract_.setNegative (
false);
236 extract_.setInputCloud (temp_cloud);
237 extract_.setIndices (inliers);
238 extract_.filter (*temp_cloud2);
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_);
252 pcl::ConvexHull<PointType> hr;
254 hr.setInputCloud(cloud_proj_);
255 cloud_hull_.reset(
new Cloud());
256 hr.reconstruct (*cloud_hull_, vertices_);
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;
268 cloud_filt_.reset(
new Cloud());
269 extract_.setNegative(
true);
270 extract_.filter(*cloud_filt_);
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_);
281 cloud_above_.reset(
new Cloud());
282 above_condrem.filter(*cloud_above_);
284 printf(
"Before: %zu After: %zu\n", cloud_filt_->points.size(),
285 cloud_above_->points.size());
288 if (! vertices_.empty()) {
289 pcl::PointIndices::Ptr polygon(
new pcl::PointIndices());
290 polygon->indices = vertices_[0].vertices;
293 pcl::ExtractIndices<PointType> polygon_extract;
295 polygon_extract.setInputCloud(cloud_hull_);
296 polygon_extract.setIndices(polygon);
297 polygon_extract.filter(polygon_cloud);
299 typename pcl::ConditionAnd<PointType>::Ptr
300 polygon_cond(
new pcl::ConditionAnd<PointType>());
302 typename PolygonComparison<PointType>::ConstPtr
303 inpoly_comp(
new PolygonComparison<PointType>(polygon_cloud));
304 polygon_cond->addComparison(inpoly_comp);
307 pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
308 condrem.setInputCloud(cloud_above_);
310 cloud_objs_.reset(
new Cloud());
311 condrem.filter(*cloud_objs_);
313 cloud_objs_.reset(
new Cloud(*cloud_above_));
320 pcl::KdTree<pcl::PointXYZRGB>::Ptr
321 kdtree_cl(
new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
322 kdtree_cl->setInputCloud(cloud_objs_);
324 std::vector<pcl::PointIndices> cluster_indices;
325 pcl::EuclideanClusterExtraction<PointType> ec;
326 ec.setClusterTolerance(0.04);
327 ec.setMinClusterSize(50);
328 ec.setMaxClusterSize(25000);
329 ec.setSearchMethod(kdtree_cl);
330 ec.setInputCloud(cloud_objs_);
331 ec.extract(cluster_indices);
333 printf(
"Found %zu clusters\n", cluster_indices.size());
335 uint8_t colors[5][3] = { {255, 0, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255},
338 std::vector<pcl::PointIndices>::const_iterator it;
339 unsigned int color = 0;
341 for (it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {
344 r = colors[color][0];
345 g = colors[color][1];
346 b = colors[color][2];
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);
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;
443 return (cloud_above_);
447 viz_cb (pcl::visualization::PCLVisualizer& viz)
451 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
456 boost::mutex::scoped_lock lock (mtx_);
458 if (!viz.updatePointCloud(cloud_proj_,
"table")) {
459 viz.addPointCloud (cloud_proj_,
"table");
461 if (!viz.updatePointCloud(cloud_objs_,
"clusters")) {
462 viz.addPointCloud (cloud_objs_,
"clusters");
465 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4,
"table");
466 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4,
"clusters");
468 viz.removeShape (
"hull");
469 if (!vertices_.empty()) {
470 viz.addPolygonMesh<PointType>(cloud_hull_, vertices_,
"hull");
487 viewer.runOnVisualizationThread(boost::bind(&OpenNIPlanarSegmentation::viz_cb,
488 this, _1),
"viz_cb");
496 while (!viewer.wasStopped ())
499 if (last_update != ct) {
504 CloudPtr cloud(
new Cloud());
505 convert_buffer_to_pcl(buf, *cloud);
509 viewer.showCloud(
get());
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_;
528 std::string device_id_;
530 CloudConstPtr cloud_;
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";
540 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
541 if (driver.getNumberDevices () > 0)
543 for (
unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
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;
553 cout <<
"No devices connected." << endl;
557 main (
int argc,
char ** argv)
565 std::string arg (argv[1]);
567 if (arg ==
"--help" || arg ==
"-h")
573 double threshold = 0.05;
574 pcl::console::parse_argument (argc, argv,
"-thresh", threshold);
579 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.