Fawkes API  Fawkes Development Version
tabletop_objects_standalone.cpp
00001 
00002 /***************************************************************************
00003  *  tabletop_objects_standalone.cpp - Thread to detect tabletop objects
00004  *
00005  *  Created: Sat Oct 01 11:51:00 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 /// @cond EXAMPLE
00023 
00024 #include <boost/thread/thread.hpp>
00025 #include <boost/date_time/posix_time/posix_time.hpp>
00026 #include <pcl/point_cloud.h>
00027 #include <pcl/point_types.h>
00028 // include <pcl/io/openni_grabber.h>
00029 #include <pcl/visualization/cloud_viewer.h>
00030 #include <pcl/io/openni_camera/openni_driver.h>
00031 #include <pcl/console/parse.h>
00032 #include <pcl/sample_consensus/method_types.h>
00033 #include <pcl/sample_consensus/model_types.h>
00034 #include <pcl/segmentation/sac_segmentation.h>
00035 #include <pcl/segmentation/extract_clusters.h>
00036 #include <pcl/filters/voxel_grid.h>
00037 #include <pcl/filters/extract_indices.h>
00038 #include <pcl/surface/concave_hull.h>
00039 #include <pcl/surface/convex_hull.h>
00040 #include <pcl/kdtree/kdtree.h>
00041 #include <pcl/kdtree/kdtree_flann.h>
00042 #include <pcl/filters/project_inliers.h>
00043 #include <pcl/filters/conditional_removal.h>
00044 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00045 
00046 #include <fvcams/shmem.h>
00047 #include <fvutils/ipc/shm_image.h>
00048 #include <fvutils/adapters/pcl.h>
00049 #include <utils/time/time.h>
00050 
00051 #define TABLE_MAX_X  3.0
00052 #define TABLE_MAX_Y  3.0
00053 #define TABLE_MIN_X -3.0
00054 #define TABLE_MIN_Y -3.0
00055 
00056 using namespace fawkes;
00057 using namespace firevision;
00058 
00059 template <typename PointT>
00060 class PolygonComparison : public pcl::ComparisonBase<PointT>
00061 {
00062   using pcl::ComparisonBase<PointT>::capable_;
00063  public:
00064   typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr;
00065   typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr;
00066  
00067   PolygonComparison(const pcl::PointCloud<PointT> &polygon, bool inside = true)
00068     : inside_(inside), polygon_(polygon)
00069   {
00070     capable_ = (polygon.size() >= 3);
00071   }
00072   virtual ~PolygonComparison() {}
00073  
00074   virtual bool evaluate(const PointT &point) const
00075   {
00076     if (inside_)
00077       return   pcl::isPointIn2DPolygon(point, polygon_);
00078     else
00079       return ! pcl::isPointIn2DPolygon(point, polygon_);
00080   }
00081  
00082  protected:
00083   bool inside_;
00084   const pcl::PointCloud<PointT> &polygon_;
00085  
00086  private:
00087   PolygonComparison() {} // not allowed
00088 };
00089 
00090 
00091 template <typename PointT>
00092 class PlaneDistanceComparison : public pcl::ComparisonBase<PointT>
00093 {
00094   using pcl::ComparisonBase<PointT>::capable_;
00095  public:
00096   typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr;
00097   typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr;
00098  
00099   PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff,
00100                           pcl::ComparisonOps::CompareOp op = pcl::ComparisonOps::GT,
00101                           float compare_val = 0.)
00102     : coeff_(coeff), op_(op), compare_val_(compare_val)
00103   {
00104     capable_ = (coeff_->values.size() == 4);
00105   }
00106   virtual ~PlaneDistanceComparison() {}
00107  
00108   virtual bool evaluate(const PointT &point) const
00109   {
00110     float val = (coeff_->values[0] * point.x + coeff_->values[1] * point.y +
00111                  coeff_->values[2] * point.z + coeff_->values[3]) /
00112       sqrtf(coeff_->values[0] * coeff_->values[0] +
00113             coeff_->values[1] * coeff_->values[1] +
00114             coeff_->values[2] * coeff_->values[2]);
00115 
00116     //printf("%f > %f?: %d\n", val, compare_val_, (val > compare_val_));
00117 
00118     if (op_ == pcl::ComparisonOps::GT) {
00119       return val >  compare_val_;
00120     } else if (op_ == pcl::ComparisonOps::GE) {
00121       return val >= compare_val_;
00122     } else if (op_ == pcl::ComparisonOps::LT) {
00123       return val <  compare_val_;
00124     } else if (op_ == pcl::ComparisonOps::LE) {
00125       return val <= compare_val_;
00126     } else {
00127       return val == compare_val_;
00128     }
00129   }
00130  
00131  protected:
00132   pcl::ModelCoefficients::ConstPtr coeff_;
00133   pcl::ComparisonOps::CompareOp op_;
00134   float compare_val_;
00135 
00136  private:
00137   PlaneDistanceComparison() {} // not allowed
00138 };
00139 
00140 template <typename PointType>
00141 class OpenNIPlanarSegmentation
00142 {
00143 public:
00144   typedef pcl::PointCloud<PointType> Cloud;
00145   typedef typename Cloud::Ptr CloudPtr;
00146   typedef typename Cloud::ConstPtr CloudConstPtr;
00147 
00148   OpenNIPlanarSegmentation (const std::string& device_id = "", double threshold = 0.01)
00149     : viewer ("PCL OpenNI Planar Segmentation Viewer"),
00150       device_id_ (device_id),
00151       new_cloud_(false)
00152   {
00153     grid_.setFilterFieldName ("z");
00154     grid_.setFilterLimits (0.0, 3.0);
00155     grid_.setLeafSize (0.01, 0.01, 0.01);
00156 
00157     seg_.setOptimizeCoefficients (true);
00158     seg_.setModelType (pcl::SACMODEL_PLANE);
00159     seg_.setMethodType (pcl::SAC_RANSAC);
00160     seg_.setMaxIterations (1000);
00161     seg_.setDistanceThreshold (threshold);
00162 
00163   }
00164 
00165   void 
00166   cloud_cb_ (const CloudConstPtr& cloud)
00167   {
00168     set (cloud);
00169   }
00170 
00171   void
00172   set (const CloudConstPtr& cloud)
00173   {
00174     //lock while we set our cloud;
00175     boost::mutex::scoped_lock lock (mtx_);
00176     cloud_  = cloud;
00177   }
00178 
00179   int get_nn(float x, float y, float z) const
00180   {
00181     PointType p(x, y, z);
00182     std::vector<int> indices;
00183     std::vector<float> distances;
00184 
00185     float min_dist = HUGE;
00186     int count = kdtree_.radiusSearch(p, 1.0, indices, distances);
00187     if (! indices.empty()) {
00188       printf("Got %i indices!\n", count);
00189       int index = 0;
00190       for (int i = 0; i < count; ++i) {
00191         if (distances[i] < min_dist) {
00192           index = i;
00193           min_dist = distances[i];
00194         }
00195       }
00196       printf("Found at dist %f (%f, %f, %f)\n", distances[index],
00197              cloud_proj_->points[indices[index]].x, 
00198              cloud_proj_->points[indices[index]].y, 
00199              cloud_proj_->points[indices[index]].z);
00200       return indices[index];
00201     } else {
00202       printf("No index found looking for (%f, %f, %f)\n", x, y, z);
00203     }
00204 
00205     return -1;
00206   }
00207 
00208   CloudPtr
00209   get ()
00210   {
00211     //lock while we swap our cloud and reset it.
00212     boost::mutex::scoped_lock lock (mtx_);
00213     CloudPtr temp_cloud (new Cloud);
00214     CloudPtr temp_cloud2 (new Cloud);
00215 
00216     grid_.setInputCloud (cloud_);
00217     grid_.filter (*temp_cloud);
00218 
00219     // set all colors to white for better distinguishing the pixels
00220     typename pcl::PointCloud<PointType>::iterator p;
00221     for (p = temp_cloud->begin(); p != temp_cloud->end(); ++p) {
00222       p->r = 255;
00223       p->g = 255;
00224       p->b = 255;
00225     }
00226 
00227     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
00228     pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
00229 
00230     seg_.setInputCloud (temp_cloud);
00231     seg_.segment (*inliers, *coefficients);
00232 
00233     extract_.setNegative (false);
00234     extract_.setInputCloud (temp_cloud);
00235     extract_.setIndices (inliers);
00236     extract_.filter (*temp_cloud2);
00237 
00238     // Project the model inliers
00239     pcl::ProjectInliers<PointType> proj;
00240     proj.setModelType(pcl::SACMODEL_PLANE);
00241     proj.setInputCloud(temp_cloud2);
00242     proj.setModelCoefficients(coefficients);
00243     cloud_proj_.reset(new Cloud());
00244     proj.filter (*cloud_proj_);
00245     //printf("PointCloud after projection has: %zu data points.\n",
00246     //       cloud_proj_->points.size());
00247 
00248 
00249     // Estimate 3D convex hull -> TABLE BOUNDARIES
00250     pcl::ConvexHull<PointType> hr;
00251     //hr.setAlpha (0.1);  // only for ConcaveHull
00252     hr.setInputCloud(cloud_proj_);
00253     cloud_hull_.reset(new Cloud());
00254     hr.reconstruct (*cloud_hull_, vertices_);
00255 
00256     //printf("Found %zu vertices, first has size %zu\n",
00257     //       vertices_.size(), vertices_[0].vertices.size());
00258 
00259     for (size_t i = 0; i < cloud_proj_->points.size(); ++i) {
00260       cloud_proj_->points[i].r =   0;
00261       cloud_proj_->points[i].g = 255;
00262       cloud_proj_->points[i].b =   0;
00263     }
00264 
00265     // Extrat all non-plane points
00266     cloud_filt_.reset(new Cloud());
00267     extract_.setNegative(true);
00268     extract_.filter(*cloud_filt_);
00269 
00270     // remove all pixels below table
00271     typename PlaneDistanceComparison<PointType>::ConstPtr
00272       above_comp(new PlaneDistanceComparison<PointType>(coefficients, pcl::ComparisonOps::LT));
00273     typename pcl::ConditionAnd<PointType>::Ptr
00274       above_cond(new pcl::ConditionAnd<PointType>());
00275     above_cond->addComparison(above_comp);
00276     pcl::ConditionalRemoval<PointType> above_condrem(above_cond);
00277     above_condrem.setInputCloud(cloud_filt_);
00278     //above_condrem.setKeepOrganized(true);
00279     cloud_above_.reset(new Cloud());
00280     above_condrem.filter(*cloud_above_);
00281 
00282     printf("Before: %zu  After: %zu\n", cloud_filt_->points.size(),
00283            cloud_above_->points.size());
00284 
00285       // Extract only points on the table plane
00286     if (! vertices_.empty()) {
00287       pcl::PointIndices::Ptr polygon(new pcl::PointIndices());
00288       polygon->indices = vertices_[0].vertices;
00289 
00290       pcl::PointCloud<PointType> polygon_cloud;
00291       pcl::ExtractIndices<PointType> polygon_extract;
00292 
00293       polygon_extract.setInputCloud(cloud_hull_);
00294       polygon_extract.setIndices(polygon);
00295       polygon_extract.filter(polygon_cloud);
00296 
00297       typename pcl::ConditionAnd<PointType>::Ptr
00298         polygon_cond(new pcl::ConditionAnd<PointType>());
00299 
00300       typename PolygonComparison<PointType>::ConstPtr
00301         inpoly_comp(new PolygonComparison<PointType>(polygon_cloud));
00302       polygon_cond->addComparison(inpoly_comp);
00303 
00304       // build the filter
00305       pcl::ConditionalRemoval<PointType> condrem(polygon_cond);
00306       condrem.setInputCloud(cloud_above_);
00307       //condrem.setKeepOrganized(true);
00308       cloud_objs_.reset(new Cloud());
00309       condrem.filter(*cloud_objs_);
00310     } else {
00311       cloud_objs_.reset(new Cloud(*cloud_above_));
00312     }
00313 
00314     // CLUSTERS
00315     // extract clusters of OBJECTS
00316 
00317     // Creating the KdTree object for the search method of the extraction
00318     pcl::KdTree<pcl::PointXYZRGB>::Ptr
00319       kdtree_cl(new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
00320     kdtree_cl->setInputCloud(cloud_objs_);
00321 
00322     std::vector<pcl::PointIndices> cluster_indices;
00323     pcl::EuclideanClusterExtraction<PointType> ec;
00324     ec.setClusterTolerance(0.04); // 2cm
00325     ec.setMinClusterSize(50);
00326     ec.setMaxClusterSize(25000);
00327     ec.setSearchMethod(kdtree_cl);
00328     ec.setInputCloud(cloud_objs_);
00329     ec.extract(cluster_indices);
00330 
00331     printf("Found %zu clusters\n", cluster_indices.size());
00332 
00333     uint8_t colors[5][3] = { {255, 0, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255},
00334                              {0, 255, 255} };
00335 
00336     std::vector<pcl::PointIndices>::const_iterator it;
00337     unsigned int color = 0;
00338     unsigned int i = 0;
00339     for (it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {
00340       uint8_t r, g, b;
00341       if (color < 5) {
00342         r = colors[color][0];
00343         g = colors[color][1];
00344         b = colors[color][2];
00345         ++color;
00346       } else {
00347         double dr=0, dg=0, db=0;
00348         pcl::visualization::getRandomColors(dr, dg, db);
00349         r = (uint8_t)roundf(dr * 255);
00350         g = (uint8_t)roundf(dg * 255);
00351         b = (uint8_t)roundf(db * 255);
00352       }
00353       printf("Cluster %u  size: %zu  color %u, %u, %u\n",
00354              ++i, it->indices.size(), r, g, b);
00355       std::vector<int>::const_iterator pit;
00356       for (pit = it->indices.begin (); pit != it->indices.end(); pit++) {
00357         cloud_objs_->points[*pit].r = r;
00358         cloud_objs_->points[*pit].g = g;
00359         cloud_objs_->points[*pit].b = b;
00360       }
00361     }
00362 
00363     /*  To project into Z:
00364     // Create a set of planar coefficients with X=Y=0,Z=1
00365     pcl::ModelCoefficients::Ptr proj_coeff (new pcl::ModelCoefficients ());
00366     proj_coeff->values.resize (4);
00367     proj_coeff->values[0] = proj_coeff->values[1] = 0;
00368     proj_coeff->values[2] = 1.0;
00369     proj_coeff->values[3] = 0;
00370 
00371     // Create the filtering object
00372     pcl::ProjectInliers<PointType> proj;
00373     proj.setModelType(pcl::SACMODEL_PLANE);
00374     proj.setInputCloud(cloud_hull_);
00375     proj.setModelCoefficients(proj_coeff);
00376     cloud_proj_.reset(new Cloud());
00377     proj.filter(*cloud_proj_);
00378 
00379     printf("Vertices: %zu, first: %zu\n", vertices_.size(), vertices_[0].vertices.size());
00380     vertices_.resize(5);
00381 
00382     //get the extents of the table
00383      if (! cloud_proj_->points.empty()) {
00384       printf("Cloud hull non-empty\n");
00385       float x_min = HUGE, y_min = HUGE, x_max = 0, y_max = 0;
00386 
00387       for (size_t i = 1; i < cloud_proj_->points.size(); ++i) {
00388         if (cloud_proj_->points[i].x < x_min && cloud_proj_->points[i].x > -3.0)
00389           x_min = cloud_proj_->points[i].x;
00390         if (cloud_proj_->points[i].x > x_max && cloud_proj_->points[i].x < 3.0)
00391           x_max = cloud_proj_->points[i].x;
00392         if (cloud_proj_->points[i].y < y_min && cloud_proj_->points[i].y > -3.0)
00393           y_min = cloud_proj_->points[i].y;
00394         if (cloud_proj_->points[i].y > y_max && cloud_proj_->points[i].y < 3.0)
00395           y_max = cloud_proj_->points[i].y;
00396       }
00397 
00398       kdtree_.setInputCloud(cloud_proj_);
00399       float table_z = cloud_proj_->points[0].z; // only need a very rough estimate
00400 
00401       printf("x_min=%f  x_max=%f  y_min=%f  y_max=%f  table_z = %f\n",
00402              x_min, x_max, y_min, y_max, table_z);
00403 
00404       int blp = get_nn(x_min, y_min, table_z);
00405       int brp = get_nn(x_max, y_min, table_z);
00406       int tlp = get_nn(x_min, y_max, table_z);
00407       int trp = get_nn(x_max, y_max, table_z);
00408 
00409       printf("blp=%i (%f,%f,%f)  brp=%i (%f,%f,%f)  "
00410              "tlp=%i (%f,%f,%f)  trp=%i (%f,%f,%f)\n",
00411              blp, cloud_proj_->points[blp].x,
00412              cloud_proj_->points[blp].y, cloud_proj_->points[blp].z,
00413              brp, cloud_proj_->points[brp].x,
00414              cloud_proj_->points[brp].y, cloud_proj_->points[brp].z,
00415              tlp, cloud_proj_->points[tlp].x,
00416              cloud_proj_->points[tlp].y, cloud_proj_->points[tlp].z,
00417              trp, cloud_proj_->points[trp].x,
00418              cloud_proj_->points[trp].y, cloud_proj_->points[trp].z);
00419 
00420       pcl::Vertices v;
00421       v.vertices.push_back(blp);
00422       v.vertices.push_back(tlp);
00423       v.vertices.push_back(trp);
00424       v.vertices.push_back(brp);
00425       v.vertices.push_back(blp);
00426       vertices_.clear();
00427       vertices_.push_back(v);
00428     }
00429 */
00430 
00431     new_cloud_ = true;
00432 
00433     // To show differences between cloud_filt and cloud_above
00434     // (draw both, increase point size of cloud_above
00435     //for (int i = 0; i < cloud_filt_->points.size(); ++i) {
00436     //  cloud_filt_->points[i].r = 255;
00437     //  cloud_filt_->points[i].g =   0;
00438     //  cloud_filt_->points[i].b =   0;
00439     //}
00440 
00441     return (cloud_above_);
00442   }
00443 
00444   void
00445   viz_cb (pcl::visualization::PCLVisualizer& viz)
00446   {
00447     if (!new_cloud_)
00448     {
00449       boost::this_thread::sleep(boost::posix_time::milliseconds(1));
00450       return;
00451     }
00452 
00453     {
00454       boost::mutex::scoped_lock lock (mtx_);
00455       // Render the data 
00456       if (!viz.updatePointCloud(cloud_proj_, "table")) {
00457         viz.addPointCloud (cloud_proj_, "table");
00458       }
00459       if (!viz.updatePointCloud(cloud_objs_, "clusters")) {
00460         viz.addPointCloud (cloud_objs_, "clusters");
00461       }
00462 
00463       viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "table");
00464       viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "clusters");
00465 
00466       viz.removeShape ("hull");
00467       if (!vertices_.empty()) {
00468         viz.addPolygonMesh<PointType>(cloud_hull_, vertices_, "hull");
00469       }
00470       new_cloud_ = false;
00471     }
00472   }
00473 
00474   void
00475   run ()
00476   {
00477     /*
00478     pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id_);
00479 
00480     boost::function<void (const CloudConstPtr&)> f =
00481       boost::bind (&OpenNIPlanarSegmentation::cloud_cb_, this, _1);
00482     boost::signals2::connection c = interface->registerCallback(f);
00483     */
00484 
00485     viewer.runOnVisualizationThread(boost::bind(&OpenNIPlanarSegmentation::viz_cb,
00486                                                 this, _1), "viz_cb");
00487 
00488     SharedMemoryCamera cam("openni-pointcloud");
00489     SharedMemoryImageBuffer *buf = cam.shared_memory_image_buffer();
00490     //interface->start ();
00491       
00492     fawkes::Time last_update;
00493 
00494     while (!viewer.wasStopped ())
00495     {
00496       fawkes::Time ct = buf->capture_time();
00497       if (last_update != ct) {
00498         last_update = ct;
00499 
00500         cam.capture();
00501 
00502         CloudPtr cloud(new Cloud());
00503         convert_buffer_to_pcl(buf, *cloud);
00504         set(cloud);
00505 
00506         //the call to get() sets the cloud_ to null;
00507         viewer.showCloud(get());
00508       }
00509     }
00510 
00511     //interface->stop ();
00512   }
00513 
00514   pcl::visualization::CloudViewer viewer;
00515   pcl::VoxelGrid<PointType> grid_;
00516   pcl::SACSegmentation<PointType> seg_;
00517   pcl::ExtractIndices<PointType> extract_;
00518   std::vector<pcl::Vertices> vertices_;
00519   CloudPtr cloud_hull_;
00520   CloudPtr cloud_proj_;
00521   CloudPtr cloud_filt_;
00522   CloudPtr cloud_above_;
00523   CloudPtr cloud_objs_;
00524   pcl::KdTreeFLANN<PointType> kdtree_;
00525 
00526   std::string device_id_;
00527   boost::mutex mtx_;
00528   CloudConstPtr cloud_;
00529   bool new_cloud_;
00530 };
00531 
00532 void
00533 usage (char ** argv)
00534 {
00535   std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n"
00536             << "where options are:\n         -thresh X        :: set the planar segmentation threshold (default: 0.5)\n";
00537 
00538   openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00539   if (driver.getNumberDevices () > 0)
00540   {
00541     for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00542     {
00543       cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
00544            << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00545       cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl
00546            << "                 bus@address for the device connected to a specific usb-bus / address combination (wotks only in Linux) or" << endl
00547            << "                 <serial-number> (only in Linux and for devices which provide serial numbers)"  << endl;
00548     }
00549   }
00550   else
00551     cout << "No devices connected." << endl;
00552 }
00553 
00554 int 
00555 main (int argc, char ** argv)
00556 {
00557   if (argc < 2)
00558   {
00559     usage (argv);
00560     return 1;
00561   }
00562 
00563   std::string arg (argv[1]);
00564   
00565   if (arg == "--help" || arg == "-h")
00566   {
00567     usage (argv);
00568     return 1;
00569   }
00570 
00571   double threshold = 0.05;
00572   pcl::console::parse_argument (argc, argv, "-thresh", threshold);
00573 
00574   //pcl::OpenNIGrabber grabber (arg);
00575   //if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
00576   //{
00577     OpenNIPlanarSegmentation<pcl::PointXYZRGB> v (arg);
00578     v.run ();
00579     /*
00580   }
00581   else
00582   {
00583     printf("Only RGBD supported atm\n");
00584     //OpenNIPlanarSegmentation<pcl::PointXYZ> v (arg, threshold);
00585     //v.run ();
00586   }
00587     */
00588 
00589   return (0);
00590 }
00591 
00592 /// @endcond EXAMPLE