openni_grabber_example.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  * Author: Suat Gedikli (gedikli@willowgarage.com)
00035  */
00036 
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/io/openni_grabber.h>
00041 #include <pcl/common/time.h>
00042 
00043 class SimpleOpenNIProcessor
00044 {
00045   public:
00046 
00047     bool save;
00048 
00049     void 
00050     cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
00051     {
00052       static unsigned count = 0;
00053       static double last = pcl::getTime ();
00054       if (++count == 30)
00055       {
00056         double now = pcl::getTime ();
00057         std::cout << "distance of center pixel :" << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << " mm. Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
00058         count = 0;
00059         last = now;
00060       }
00061 
00062       if (save)
00063       {
00064         std::stringstream ss;
00065         ss << std::setprecision(12) << pcl::getTime () * 100 << ".pcd";
00066         pcl::io::savePCDFile (ss.str (), *cloud);
00067         std::cout << "wrote point clouds to file " << ss.str() << std::endl;
00068       }
00069     }
00070 
00071     void 
00072     imageDepthImageCallback (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>& d_img, float constant)
00073     {
00074       static unsigned count = 0;
00075       static double last = pcl::getTime ();
00076       if (++count == 30)
00077       {
00078         double now = pcl::getTime ();
00079         std::cout << "got synchronized image x depth-image with constant factor: " << constant << ". Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
00080         std::cout << "Depth baseline: " << d_img->getBaseline () << " and focal length: " << d_img->getFocalLength () << std::endl;
00081         count = 0;
00082         last = now;
00083       }
00084     }
00085 
00086     void run ()
00087     {
00088       save = false;
00089 
00090       // create a new grabber for OpenNI devices
00091       pcl::Grabber* interface = new pcl::OpenNIGrabber();
00092 
00093       // make callback function from member function
00094       boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
00095         boost::bind (&SimpleOpenNIProcessor::cloud_cb_, this, _1);
00096 
00097       // connect callback function for desired signal. In this case its a point cloud with color values
00098       boost::signals2::connection c = interface->registerCallback (f);
00099 
00100       // make callback function from member function
00101       boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant)> f2 =
00102         boost::bind (&SimpleOpenNIProcessor::imageDepthImageCallback, this, _1, _2, _3);
00103 
00104       // connect callback function for desired signal. In this case its a point cloud with color values
00105       boost::signals2::connection c2 = interface->registerCallback (f2);
00106 
00107       // start receiving point clouds
00108       interface->start ();
00109 
00110       std::cout << "<Esc>, \'q\', \'Q\': quit the program" << std::endl;
00111       std::cout << "\' \': pause" << std::endl;
00112       std::cout << "\'s\': save" << std::endl;
00113       char key;
00114       do
00115       {
00116         key = static_cast<char> (getchar ());
00117         switch (key)
00118         {
00119           case ' ':
00120             if (interface->isRunning())
00121               interface->stop();
00122             else
00123               interface->start();
00124           case 's':
00125             save = !save;
00126         }
00127       } while (key != 27 && key != 'q' && key != 'Q');
00128 
00129       // stop the grabber
00130       interface->stop ();
00131     }
00132 };
00133 
00134 int
00135 main ()
00136 {
00137   SimpleOpenNIProcessor v;
00138   v.run ();
00139   return 0;
00140 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:01