openni_save_image.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *      
00036  */
00037 
00038 #include <boost/thread/thread.hpp>
00039 #include <boost/date_time/posix_time/posix_time.hpp>
00040 #include <pcl/common/time.h> //fps calculations
00041 #include <pcl/io/openni_grabber.h>
00042 #include <pcl/io/openni_camera/openni_driver.h>
00043 #include <pcl/console/parse.h>
00044 
00045 #include <vector>
00046 #include <string>
00047 
00048 #include <pcl/visualization/vtk.h>
00049 #include <pcl/visualization/pcl_visualizer.h>
00050 
00051 #define SHOW_FPS 1
00052 #if SHOW_FPS
00053 #define FPS_CALC(_WHAT_) \
00054 do \
00055 { \
00056     static unsigned count = 0;\
00057     static double last = pcl::getTime ();\
00058     double now = pcl::getTime (); \
00059     ++count; \
00060     if (now - last >= 1.0) \
00061     { \
00062       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
00063       count = 0; \
00064       last = now; \
00065     } \
00066 }while(false)
00067 #else
00068 #define FPS_CALC(_WHAT_) \
00069 do \
00070 { \
00071 }while(false)
00072 #endif
00073 
00075 class SimpleOpenNIViewer
00076 {
00077   public:
00078     SimpleOpenNIViewer (pcl::OpenNIGrabber& grabber)
00079       : grabber_ (grabber)
00080       , image_mutex_ ()
00081       , image_ ()
00082       , depth_image_ ()
00083       , importer_ (vtkSmartPointer<vtkImageImport>::New ())
00084       , depth_importer_ (vtkSmartPointer<vtkImageImport>::New ())
00085       , writer_ (vtkSmartPointer<vtkTIFFWriter>::New ())
00086       , flipper_ (vtkSmartPointer<vtkImageFlip>::New ())
00087     {
00088       importer_->SetNumberOfScalarComponents (3);
00089       importer_->SetDataScalarTypeToUnsignedChar ();
00090       depth_importer_->SetNumberOfScalarComponents (1);
00091       depth_importer_->SetDataScalarTypeToUnsignedShort ();
00092       writer_->SetCompressionToPackBits ();
00093       flipper_->SetFilteredAxes (1);
00094     }
00095 
00096     void
00097     image_callback (const boost::shared_ptr<openni_wrapper::Image> &image, 
00098                     const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image, float)
00099     {
00100       FPS_CALC ("image callback");
00101       boost::mutex::scoped_lock lock (image_mutex_);
00102       image_ = image;
00103       depth_image_ = depth_image;
00104     }
00105     
00106     void
00107     run ()
00108     {
00109       boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float) > image_cb = boost::bind (&SimpleOpenNIViewer::image_callback, this, _1, _2, _3);
00110       boost::signals2::connection image_connection = grabber_.registerCallback (image_cb);
00111       
00112       grabber_.start ();
00113       
00114       unsigned char* rgb_data = 0;
00115       unsigned rgb_data_size = 0;
00116       const void* data;
00117        
00118       while (true)
00119       {
00120         boost::mutex::scoped_lock lock (image_mutex_);
00121 
00122         std::string time = boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ());
00123         if (image_)
00124         {
00125           FPS_CALC ("writer callback");
00126           boost::shared_ptr<openni_wrapper::Image> image;
00127           image.swap (image_);
00128 
00129           if (image->getEncoding() == openni_wrapper::Image::RGB)
00130           {
00131             data = reinterpret_cast<const void*> (image->getMetaData ().Data ());
00132             importer_->SetWholeExtent (0, image->getWidth () - 1, 0, image->getHeight () - 1, 0, 0);
00133             importer_->SetDataExtentToWholeExtent ();
00134           }
00135           else
00136           {
00137             if (rgb_data_size < image->getWidth () * image->getHeight ())
00138             {
00139               rgb_data_size = image->getWidth () * image->getHeight ();
00140               rgb_data = new unsigned char [rgb_data_size * 3];
00141               importer_->SetWholeExtent (0, image->getWidth () - 1, 0, image->getHeight () - 1, 0, 0);
00142               importer_->SetDataExtentToWholeExtent ();
00143             }
00144             image->fillRGB (image->getWidth (), image->getHeight (), rgb_data);
00145             data = reinterpret_cast<const void*> (rgb_data);
00146           }
00147 
00148           std::stringstream ss;
00149           ss << "frame_" + time + "_rgb.tiff";
00150           importer_->SetImportVoidPointer (const_cast<void*>(data), 1);
00151           importer_->Update ();
00152           flipper_->SetInputConnection (importer_->GetOutputPort ());
00153           flipper_->Update ();
00154           writer_->SetFileName (ss.str ().c_str ());
00155           writer_->SetInputConnection (flipper_->GetOutputPort ());
00156           writer_->Write ();
00157         }
00158 
00159         if (depth_image_)
00160         {
00161           boost::shared_ptr<openni_wrapper::DepthImage> depth_image;
00162           depth_image.swap (depth_image_);
00163 
00164           std::stringstream ss;
00165           ss << "frame_" + time + "_depth.tiff";
00166 
00167           depth_importer_->SetWholeExtent (0, depth_image->getWidth () - 1, 0, depth_image->getHeight () - 1, 0, 0);
00168           depth_importer_->SetDataExtentToWholeExtent ();
00169           depth_importer_->SetImportVoidPointer (const_cast<void*> (reinterpret_cast<const void*> (depth_image->getDepthMetaData ().Data ())), 1);
00170           depth_importer_->Update ();
00171           flipper_->SetInputConnection (depth_importer_->GetOutputPort ());
00172           flipper_->Update ();
00173           writer_->SetFileName (ss.str ().c_str ());
00174           writer_->SetInputConnection (flipper_->GetOutputPort ());
00175           writer_->Write ();
00176         }
00177       }
00178 
00179       grabber_.stop ();
00180       
00181       image_connection.disconnect ();
00182       
00183       if (rgb_data)
00184         delete[] rgb_data;
00185     }
00186 
00187     pcl::OpenNIGrabber& grabber_;
00188     boost::mutex image_mutex_;
00189     boost::shared_ptr<openni_wrapper::Image> image_;
00190     boost::shared_ptr<openni_wrapper::DepthImage> depth_image_;
00191     vtkSmartPointer<vtkImageImport> importer_, depth_importer_;
00192     vtkSmartPointer<vtkTIFFWriter> writer_;
00193     vtkSmartPointer<vtkImageFlip> flipper_;
00194 };
00195 
00196 void
00197 usage (char ** argv)
00198 {
00199   cout << "usage: " << argv[0] << " [((<device_id> | <path-to-oni-file>) [-imagemode <mode>] | -l [<device_id>]| -h | --help)]" << endl;
00200   cout << argv[0] << " -h | --help : shows this help" << endl;
00201   cout << argv[0] << " -l : list all available devices" << endl;
00202   cout << argv[0] << " -l <device-id> : list all available modes for specified device" << endl;
00203 
00204   cout << "                 device_id may be #1, #2, ... for the first, second etc device in the list"
00205 #ifndef _WIN32
00206        << " or" << endl
00207        << "                 bus@address for the device connected to a specific usb-bus / address combination or" << endl
00208        << "                 <serial-number>"
00209 #endif
00210        << endl;
00211   cout << endl;
00212   cout << "examples:" << endl;
00213   cout << argv[0] << " \"#1\"" << endl;
00214   cout << "    uses the first device." << endl;
00215   cout << argv[0] << " \"./temp/test.oni\"" << endl;
00216   cout << "    uses the oni-player device to play back oni file given by path." << endl;
00217   cout << argv[0] << " -l" << endl;
00218   cout << "    lists all available devices." << endl;
00219   cout << argv[0] << " -l \"#2\"" << endl;
00220   cout << "    lists all available modes for the second device" << endl;
00221 #ifndef _WIN32
00222   cout << argv[0] << " A00361800903049A" << endl;
00223   cout << "    uses the device with the serial number \'A00361800903049A\'." << endl;
00224   cout << argv[0] << " 1@16" << endl;
00225   cout << "    uses the device on address 16 at USB bus 1." << endl;
00226 #endif
00227   return;
00228 }
00229 
00230 int
00231 main(int argc, char ** argv)
00232 {
00233   std::string device_id ("");
00234   pcl::OpenNIGrabber::Mode image_mode = pcl::OpenNIGrabber::OpenNI_Default_Mode;
00235   
00236   if (argc >= 2)
00237   {
00238     device_id = argv[1];
00239     if (device_id == "--help" || device_id == "-h")
00240     {
00241       usage(argv);
00242       return 0;
00243     }
00244     else if (device_id == "-l")
00245     {
00246       if (argc >= 3)
00247       {
00248         pcl::OpenNIGrabber grabber (argv[2]);
00249         boost::shared_ptr<openni_wrapper::OpenNIDevice> device = grabber.getDevice ();
00250         std::vector<std::pair<int, XnMapOutputMode> > modes;
00251 
00252         if (device->hasImageStream ())
00253         {
00254           cout << endl << "Supported image modes for device: " << device->getVendorName () << " , " << device->getProductName () << endl;
00255           modes = grabber.getAvailableImageModes ();
00256           for (std::vector<std::pair<int, XnMapOutputMode> >::const_iterator it = modes.begin (); it != modes.end (); ++it)
00257           {
00258             cout << it->first << " = " << it->second.nXRes << " x " << it->second.nYRes << " @ " << it->second.nFPS << endl;
00259           }
00260         }
00261       }
00262       else
00263       {
00264         openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00265         if (driver.getNumberDevices () > 0)
00266         {
00267           for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00268           {
00269             cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
00270               << ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00271           }
00272 
00273         }
00274         else
00275           cout << "No devices connected." << endl;
00276         
00277         cout <<"Virtual Devices available: ONI player" << endl;
00278       }
00279       return (0);
00280     }
00281   }
00282   else
00283   {
00284     openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00285     if (driver.getNumberDevices () > 0)
00286       cout << "Device Id not set, using first device." << endl;
00287   }
00288   
00289   unsigned mode;
00290   if (pcl::console::parse (argc, argv, "-imagemode", mode) != -1)
00291     image_mode = pcl::OpenNIGrabber::Mode (mode);
00292   
00293   pcl::OpenNIGrabber grabber (device_id, pcl::OpenNIGrabber::OpenNI_Default_Mode, image_mode);
00294   SimpleOpenNIViewer v (grabber);
00295   v.run ();
00296 
00297   return (0);
00298 }


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