oni_viewer_simple.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: Nico Blodow (blodow@cs.tum.edu)
00035  *         Radu Bogdan Rusu (rusu@willowgarage.com)
00036  *         Suat Gedikli (gedikli@willowgarage.com)
00037  *         Ethan Rublee (rublee@willowgarage.com)
00038  */
00039 
00040 #include <boost/thread/thread.hpp>
00041 #include <boost/date_time/posix_time/posix_time.hpp>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/common/time.h> //fps calculations
00045 #include <pcl/io/oni_grabber.h>
00046 #include <pcl/visualization/cloud_viewer.h>
00047 #include <vector>
00048 #include <pcl/common/time_trigger.h>
00049 
00050 #define SHOW_FPS 1
00051 #if SHOW_FPS
00052 #define FPS_CALC(_WHAT_) \
00053 do \
00054 { \
00055     static unsigned count = 0;\
00056     static double last = pcl::getTime ();\
00057     ++count; \
00058     if (pcl::getTime() - last >= 1.0) \
00059     { \
00060       double now = pcl::getTime (); \
00061       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
00062       count = 0; \
00063       last = now; \
00064     } \
00065 }while(false)
00066 #else
00067 #define FPS_CALC(_WHAT_) \
00068 do \
00069 { \
00070 }while(false)
00071 #endif
00072 
00073 template <typename PointType>
00074 class SimpleONIViewer
00075 {
00076 public:
00077   typedef pcl::PointCloud<PointType> Cloud;
00078   typedef typename Cloud::ConstPtr CloudConstPtr;
00079 
00080   SimpleONIViewer(pcl::ONIGrabber& grabber)
00081     : viewer("PCL OpenNI Viewer")
00082     , grabber_(grabber)
00083   {
00084   }
00085 
00090   void
00091   cloud_cb_ (const CloudConstPtr& cloud)
00092   {
00093     FPS_CALC ("callback");
00094     boost::mutex::scoped_lock lock (mtx_);
00095     cloud_ = cloud;
00096   }
00097 
00102   CloudConstPtr
00103   getLatestCloud ()
00104   {
00105     //lock while we swap our cloud and reset it.
00106     boost::mutex::scoped_lock lock(mtx_);
00107     CloudConstPtr temp_cloud;
00108     temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
00109     //it is safe to set it again from our
00110     //callback
00111     return (temp_cloud);
00112   }
00113 
00117   void
00118   run()
00119   {
00120     //pcl::Grabber* interface = new pcl::OpenNIGrabber(device_id_, pcl::OpenNIGrabber::OpenNI_QQVGA_30Hz, pcl::OpenNIGrabber::OpenNI_VGA_30Hz);
00121 
00122     boost::function<void (const CloudConstPtr&) > f = boost::bind (&SimpleONIViewer::cloud_cb_, this, _1);
00123 
00124     boost::signals2::connection c = grabber_.registerCallback (f);
00125 
00126     grabber_.start();
00127 
00128     while (!viewer.wasStopped ())
00129     {
00130       if (cloud_)
00131       {
00132         FPS_CALC ("drawing");
00133         //the call to get() sets the cloud_ to null;
00134         viewer.showCloud (getLatestCloud ());
00135       }
00136     }
00137 
00138     grabber_.stop();
00139   }
00140 
00141   pcl::visualization::CloudViewer viewer;
00142   pcl::ONIGrabber& grabber_;
00143   boost::mutex mtx_;
00144   CloudConstPtr cloud_;
00145 };
00146 
00147 void
00148 usage(char ** argv)
00149 {
00150   cout << "usage: " << argv[0] << " <path-to-oni-file> [framerate]\n";
00151   cout << argv[0] << " -h | --help : shows this help" << endl;
00152   return;
00153 }
00154 
00155 int
00156 main(int argc, char ** argv)
00157 {
00158   std::string arg("");
00159 
00160   unsigned frame_rate = 0;
00161   if (argc >= 2)
00162   {
00163     arg = argv[1];
00164 
00165     if (arg == "--help" || arg == "-h")
00166     {
00167       usage(argv);
00168       return 1;
00169     }
00170     
00171     if (argc >= 3)
00172     {
00173       frame_rate = atoi(argv[2]);
00174     }
00175   }
00176   else
00177   {
00178     usage (argv);
00179     return 1;
00180   }
00181 
00182   pcl::TimeTrigger trigger;
00183   
00184   pcl::ONIGrabber* grabber = 0;
00185   if (frame_rate == 0)
00186     grabber = new  pcl::ONIGrabber(arg, true, true);
00187   else
00188   {
00189     grabber = new  pcl::ONIGrabber(arg, true, false);
00190     trigger.setInterval (1.0 / (double) frame_rate);
00191     trigger.registerCallback (boost::bind(&pcl::ONIGrabber::start, grabber));
00192     trigger.start();
00193   }
00194   if (grabber->providesCallback<pcl::ONIGrabber::sig_cb_openni_point_cloud_rgb > ())
00195   {
00196     SimpleONIViewer<pcl::PointXYZRGBA> v(*grabber);
00197     v.run();
00198   }
00199   else
00200   {
00201     SimpleONIViewer<pcl::PointXYZI> v(*grabber);
00202     v.run();
00203   }
00204 
00205   return (0);
00206 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:58