hdl_viewer_simple.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, The MITRE Corporation
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: Keven Ring <keven@mitre.org>
00035  */
00036 
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl/common/time.h> //fps calculations
00040 #include <pcl/io/hdl_grabber.h>
00041 #include <pcl/visualization/point_cloud_color_handlers.h>
00042 #include <pcl/visualization/cloud_viewer.h>
00043 #include <pcl/visualization/image_viewer.h>
00044 #include <pcl/io/openni_camera/openni_driver.h>
00045 #include <pcl/console/parse.h>
00046 #include <pcl/visualization/boost.h>
00047 #include <pcl/visualization/mouse_event.h>
00048 #include <vector>
00049 #include <string>
00050 #include <boost/algorithm/string.hpp>
00051 #include <typeinfo>
00052 
00053 using namespace std;
00054 using namespace pcl;
00055 using namespace pcl::console;
00056 using namespace pcl::visualization;
00057 
00058 #define SHOW_FPS 0
00059 #if SHOW_FPS
00060 #define FPS_CALC(_WHAT_) \
00061 do \
00062 { \
00063     static unsigned count = 0;\
00064     static double last = getTime ();\
00065     double now = getTime (); \
00066     ++count; \
00067     if (now - last >= 1.0) \
00068     { \
00069       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
00070       count = 0; \
00071       last = now; \
00072     } \
00073 }while(false)
00074 #else
00075 #define FPS_CALC(_WHAT_) \
00076 do \
00077 { \
00078 }while(false)
00079 #endif
00080 
00082 template<typename PointType>
00083 class SimpleHDLViewer
00084 {
00085   public:
00086     typedef PointCloud<PointType> Cloud;
00087     typedef typename Cloud::ConstPtr CloudConstPtr;
00088 
00089     SimpleHDLViewer (Grabber& grabber,
00090                      PointCloudColorHandler<PointType> &handler) 
00091       : cloud_viewer_ (new PCLVisualizer ("PCL HDL Cloud"))
00092       , grabber_ (grabber)
00093       , handler_ (handler)
00094     {
00095     }
00096 
00097     void 
00098     cloud_callback (const CloudConstPtr& cloud)
00099     {
00100       FPS_CALC ("cloud callback");
00101       boost::mutex::scoped_lock lock (cloud_mutex_);
00102       cloud_ = cloud;
00103       //std::cout << cloud->points[0] << " " << cloud->size () << std::endl;
00104     }
00105 
00106     void 
00107     cloud_callback (const CloudConstPtr& cloud, float startAngle,
00108                     float endAngle)
00109     {
00110       FPS_CALC ("cloud callback");
00111       boost::mutex::scoped_lock lock (cloud_mutex_);
00112       cloud_ = cloud;
00113     }
00114 
00115     void 
00116     keyboard_callback (const KeyboardEvent& event,
00117                        void* cookie)
00118     {
00119       if (event.keyUp ())
00120       {
00121         return;
00122       }
00123     }
00124 
00125     void 
00126     mouse_callback (const MouseEvent& mouse_event,
00127                     void* cookie)
00128     {
00129       if (mouse_event.getType () == MouseEvent::MouseButtonPress && 
00130           mouse_event.getButton () == MouseEvent::LeftButton)
00131       {
00132         cout << mouse_event.getX () << " , " << mouse_event.getY () << endl;
00133       }
00134     }
00135 
00136     void 
00137     run ()
00138     {
00139       cloud_viewer_->addCoordinateSystem (3.0);
00140       cloud_viewer_->setBackgroundColor (0, 0, 0);
00141       cloud_viewer_->initCameraParameters ();
00142       cloud_viewer_->setCameraPosition (0.0, 0.0, 30.0, 0.0, 1.0, 0.0, 0);
00143       cloud_viewer_->setCameraClipDistances (0.0, 50.0);
00144       //cloud_viewer_->registerMouseCallback(&SimpleHDLViewer::mouse_callback, *this);
00145       //cloud_viewer_->registerKeyboardCallback (&SimpleHDLViewer::keyboard_callback, *this);
00146 
00147       //boost::function<void(const CloudConstPtr&, float, float)> cloud_cb = boost::bind(&SimpleHDLViewer::cloud_callback, this, _1, _2, _3);
00148       boost::function<void (const CloudConstPtr&)> cloud_cb = boost::bind (
00149           &SimpleHDLViewer::cloud_callback, this, _1);
00150       boost::signals2::connection cloud_connection = grabber_.registerCallback (
00151           cloud_cb);
00152 
00153       grabber_.start ();
00154 
00155       while (!cloud_viewer_->wasStopped ())
00156       {
00157         CloudConstPtr cloud;
00158 
00159         // See if we can get a cloud
00160         if (cloud_mutex_.try_lock ())
00161         {
00162           cloud_.swap (cloud);
00163           cloud_mutex_.unlock ();
00164         }
00165 
00166         if (cloud)
00167         {
00168           FPS_CALC("drawing cloud");
00169           handler_.setInputCloud (cloud);
00170           if (!cloud_viewer_->updatePointCloud (cloud, handler_, "HDL"))
00171             cloud_viewer_->addPointCloud (cloud, handler_, "HDL");
00172 
00173           cloud_viewer_->spinOnce ();
00174         }
00175 
00176         if (!grabber_.isRunning ())
00177           cloud_viewer_->spin ();
00178 
00179         boost::this_thread::sleep (boost::posix_time::microseconds (100));
00180       }
00181 
00182       grabber_.stop ();
00183 
00184       cloud_connection.disconnect ();
00185     }
00186 
00187     boost::shared_ptr<PCLVisualizer> cloud_viewer_;
00188     boost::shared_ptr<ImageViewer> image_viewer_;
00189 
00190     Grabber& grabber_;
00191     boost::mutex cloud_mutex_;
00192     boost::mutex image_mutex_;
00193 
00194     CloudConstPtr cloud_;
00195     PointCloudColorHandler<PointType> &handler_;
00196 };
00197 
00198 void
00199 usage (char ** argv)
00200 {
00201   cout << "usage: " << argv[0]
00202       << " [-hdlCalibration <path-to-calibration-file>] [-pcapFile <path-to-pcap-file>] [-h | --help] [-format XYZ(default)|XYZI|XYZRGB]"
00203       << endl;
00204   cout << argv[0] << " -h | --help : shows this help" << endl;
00205   return;
00206 }
00207 
00208 int 
00209 main (int argc, char ** argv)
00210 {
00211   std::string hdlCalibration, pcapFile, format ("XYZ");
00212 
00213   if (find_switch (argc, argv, "-h") || 
00214       find_switch (argc, argv, "--help"))
00215   {
00216     usage (argv);
00217     return (0);
00218   }
00219 
00220   parse_argument (argc, argv, "-calibrationFile", hdlCalibration);
00221   parse_argument (argc, argv, "-pcapFile", pcapFile);
00222   parse_argument (argc, argv, "-format", format);
00223 
00224   HDLGrabber grabber (hdlCalibration, pcapFile);
00225 
00226   cout << "viewer format:" << format << endl;
00227   if (boost::iequals (format, std::string ("XYZ")))
00228   {
00229     std::vector<double> fcolor (3); fcolor[0] = fcolor[1] = fcolor[2] = 255.0;
00230     pcl::console::parse_3x_arguments (argc, argv, "-fc", fcolor[0], fcolor[1], fcolor[2]);
00231     PointCloudColorHandlerCustom<PointXYZ> color_handler (fcolor[0], fcolor[1], fcolor[2]);
00232 
00233     SimpleHDLViewer<PointXYZ> v (grabber, color_handler);
00234     v.run ();
00235   }
00236   else if (boost::iequals (format, std::string ("XYZI")))
00237   {
00238     PointCloudColorHandlerGenericField<PointXYZI> color_handler ("intensity");
00239 
00240     SimpleHDLViewer<PointXYZI> v (grabber, color_handler);
00241     v.run ();
00242   }
00243   else if (boost::iequals (format, std::string ("XYZRGB")))
00244   {
00245     PointCloudColorHandlerRGBField<PointXYZRGBA> color_handler;
00246 
00247     SimpleHDLViewer<PointXYZRGBA> v (grabber, color_handler);
00248     v.run ();
00249   }
00250   return (0);
00251 }
00252 


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