openni_3d_concave_hull.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *      
00034  */
00035 
00036 #include <pcl/point_cloud.h>
00037 #include <pcl/point_types.h>
00038 #include <pcl/io/openni_grabber.h>
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/io/openni_camera/openni_driver.h>
00041 #include <pcl/filters/voxel_grid.h>
00042 #include <pcl/surface/concave_hull.h>
00043 #include <pcl/console/parse.h>
00044 #include <pcl/common/time.h>
00045 #include <pcl/visualization/cloud_viewer.h>
00046 
00047 using namespace pcl;
00048 using namespace pcl::visualization;
00049 using namespace std;
00050 
00051 #define FPS_CALC(_WHAT_) \
00052 do \
00053 { \
00054     static unsigned count = 0;\
00055     static double last = pcl::getTime ();\
00056     if (++count == 100) \
00057     { \
00058       double now = pcl::getTime (); \
00059       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
00060       count = 0; \
00061       last = now; \
00062     } \
00063 }while(false)
00064 
00065 template <typename PointType>
00066 class OpenNI3DConcaveHull
00067 {
00068   public:
00069     typedef pcl::PointCloud<PointType> Cloud;
00070     typedef typename Cloud::Ptr CloudPtr;
00071     typedef typename Cloud::ConstPtr CloudConstPtr;
00072 
00073     OpenNI3DConcaveHull (const std::string& device_id = "")
00074       : viewer ("PCL OpenNI 3D Concave Hull Viewer") 
00075     , device_id_(device_id)
00076     {
00077       grid.setFilterFieldName ("z");
00078       grid.setFilterLimits (0.0, 1.0);
00079       grid.setLeafSize (0.01f, 0.01f, 0.01f);
00080     }
00081     
00082     void 
00083     cloud_cb (const CloudConstPtr& cloud)
00084     {
00085       boost::mutex::scoped_lock lock (mtx_);
00086       FPS_CALC ("computation");
00087 
00088       cloud_pass_.reset (new Cloud);
00089       // Computation goes here
00090       grid.setInputCloud (cloud);
00091       grid.filter (*cloud_pass_);
00092 
00093       // Estimate 3D convex hull
00094       pcl::ConcaveHull<PointType> hr;
00095       hr.setAlpha (0.1);
00096       hr.setInputCloud (cloud_pass_);
00097       cloud_hull_.reset (new Cloud);
00098       hr.reconstruct (*cloud_hull_, vertices_);
00099 
00100       cloud_ = cloud;
00101       new_cloud_ = true;
00102     }
00103 
00104     void
00105     viz_cb (pcl::visualization::PCLVisualizer& viz)
00106     {
00107       if (!cloud_ || !new_cloud_)
00108       {
00109         boost::this_thread::sleep (boost::posix_time::milliseconds (1));
00110         return;
00111       }
00112 
00113       {
00114         boost::mutex::scoped_lock lock (mtx_);
00115         FPS_CALC ("visualization");
00116         CloudPtr temp_cloud;
00117         temp_cloud.swap (cloud_pass_);
00118 
00119         if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
00120         {
00121           viz.addPointCloud (temp_cloud, "OpenNICloud");
00122           viz.resetCameraViewpoint ("OpenNICloud");
00123         }
00124         // Render the data 
00125         if (new_cloud_ && cloud_hull_)
00126         {
00127           viz.removePointCloud ("hull");
00128           viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
00129         }
00130         new_cloud_ = false;
00131       }
00132     }
00133 
00134     void
00135     run ()
00136     {
00137       pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id_);
00138 
00139       boost::function<void (const CloudConstPtr&)> f = boost::bind (&OpenNI3DConcaveHull::cloud_cb, this, _1);
00140       boost::signals2::connection c = interface->registerCallback (f);
00141      
00142       viewer.runOnVisualizationThread (boost::bind(&OpenNI3DConcaveHull::viz_cb, this, _1), "viz_cb");
00143 
00144       interface->start ();
00145       
00146       while (!viewer.wasStopped ())
00147       {
00148         boost::this_thread::sleep(boost::posix_time::milliseconds(1));
00149       }
00150 
00151       interface->stop ();
00152     }
00153 
00154     pcl::VoxelGrid<PointType> grid;
00155     pcl::visualization::CloudViewer viewer;
00156 
00157     std::string device_id_;
00158     boost::mutex mtx_;
00159     // Data
00160     CloudConstPtr cloud_;
00161     CloudPtr cloud_pass_, cloud_hull_;
00162     std::vector<pcl::Vertices> vertices_;
00163     bool new_cloud_;
00164 };
00165 
00166 void
00167 usage (char ** argv)
00168 {
00169   std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
00170 
00171   openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00172   if (driver.getNumberDevices () > 0)
00173   {
00174     for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00175     {
00176       cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
00177               << ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00178       cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl
00179            << "                 bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << endl
00180            << "                 <serial-number> (only in Linux and for devices which provide serial numbers)"  << endl;
00181     }
00182   }
00183   else
00184     cout << "No devices connected." << endl;
00185 }
00186 
00187 int
00188 main (int argc, char ** argv)
00189 {
00190   std::string arg;
00191   if (argc > 1)
00192     arg = std::string (argv[1]);
00193   
00194   if (arg == "--help" || arg == "-h")
00195   {
00196     usage (argv);
00197     return 1;
00198   }
00199 
00200   pcl::OpenNIGrabber grabber ("");
00201   if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
00202   {
00203     PCL_INFO ("PointXYZRGBA mode enabled.\n");
00204     OpenNI3DConcaveHull<pcl::PointXYZRGBA> v ("");
00205     v.run ();
00206   }
00207   else
00208   {
00209     PCL_INFO ("PointXYZ mode enabled.\n");
00210     OpenNI3DConcaveHull<pcl::PointXYZ> v ("");
00211     v.run ();
00212   }
00213   return (0);
00214 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:20