openni_example.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 2011 Willow Garage, Inc.
00005  *    Suat Gedikli <gedikli@willowgarage.com>
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 #include <openni_camera/openni_driver.h>
00038 #include <openni_camera/openni_device.h>
00039 #include <openni_camera/openni_image.h>
00040 #include <openni_camera/openni_depth_image.h>
00041 #include <iostream>
00042 #include <string>
00043 #include <map>
00044 #include <XnCppWrapper.h>
00045 #include <opencv2/opencv.hpp>
00046 #include <boost/thread.hpp>
00047 #include <sys/times.h>
00048 
00049 using namespace std;
00050 using namespace openni_wrapper;
00051 using namespace cv;
00052 using namespace boost;
00053 
00054 class MyOpenNIExample
00055 {
00056 public:
00057 
00058   typedef struct ImgContext : public boost::noncopyable
00059   {
00060 
00061     ImgContext () : is_new (false)
00062     {
00063     }
00064 
00065     ImgContext (const Mat & img) : image (img), is_new (false)
00066     {
00067     }
00068     Mat image;
00069     mutable boost::mutex lock;
00070     bool is_new;
00071   } ImageContext;
00072 public:
00073   MyOpenNIExample (const vector<unsigned>& device_indices);
00074   ~MyOpenNIExample ();
00075   int run ();
00076 private:
00077   void imageCallback (boost::shared_ptr<Image> image, void* cookie);
00078   void depthCallback (boost::shared_ptr<DepthImage> depth, void* cookie);
00079   void writeImages () const;
00080   map<string, ImageContext*> rgb_images_;
00081   map<string, ImageContext*> gray_images_;
00082   map<string, ImageContext*> depth_images_;
00083   vector< boost::shared_ptr<OpenNIDevice> > devices_;
00084   bool running_;
00085   unsigned selected_device_;
00086 
00087   double image_timestamp;
00088   double depth_timestamp;
00089 };
00090 
00091 MyOpenNIExample::MyOpenNIExample (const vector<unsigned>& device_indices)
00092 : running_ (false)
00093 , selected_device_ (0)
00094 {
00095   OpenNIDriver& driver = OpenNIDriver::getInstance ();
00096 
00097   for (vector<unsigned>::const_iterator indexIt = device_indices.begin (); indexIt != device_indices.end (); ++indexIt)
00098   {
00099     if (*indexIt >= driver.getNumberDevices ())
00100     {
00101       cout << "Index out of range." << driver.getNumberDevices () << " devices found." << endl;
00102       exit (1);
00103     }
00104 
00105     boost::shared_ptr<OpenNIDevice> device = driver.getDeviceByIndex (*indexIt);
00106     cout << devices_.size () + 1 << ". device on bus: " << (int)device->getBus () << " @ " << (int)device->getAddress ()
00107             << " with serial number: " << device->getSerialNumber () << "  "
00108             << device->getVendorName () << " : " << device->getProductName () << endl;
00109     devices_.push_back (device);
00110 
00111     const int width = 640;
00112     const int height = 480;
00113     XnMapOutputMode mode;
00114     mode.nXRes = width;
00115     mode.nYRes = height;
00116     mode.nFPS = 30;
00117     
00118     if (device->hasImageStream())
00119     {
00120       if (!device->isImageModeSupported (mode))
00121       {
00122         cout << "image stream mode " << mode.nXRes << " x " << mode.nYRes << " @ " << mode.nFPS << " not supported" << endl;
00123         exit (-1);
00124       }
00125       namedWindow (string (device->getConnectionString ()) + "RGB", WINDOW_AUTOSIZE);
00126       namedWindow (string (device->getConnectionString ()) + "Gray", WINDOW_AUTOSIZE);
00127       rgb_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_8UC3));
00128       gray_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_8UC1));
00129       device->registerImageCallback (&MyOpenNIExample::imageCallback, *this, &(*device));
00130     }
00131     if (device->hasDepthStream())
00132     {
00133       if (!device->isDepthModeSupported (mode))
00134       {
00135         cout << "depth stream mode " << mode.nXRes << " x " << mode.nYRes << " @ " << mode.nFPS << " not supported" << endl;
00136         exit (-1);
00137       }
00138       namedWindow (string (device->getConnectionString ()) + "Depth", WINDOW_AUTOSIZE);
00139       depth_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_32FC1));
00140       device->registerDepthCallback (&MyOpenNIExample::depthCallback, *this, &(*device));
00141     }
00142   }
00143 
00144   timeval timestamp;
00145   gettimeofday (&timestamp, NULL);
00146   image_timestamp = depth_timestamp = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
00147 }
00148 
00149 MyOpenNIExample::~MyOpenNIExample ()
00150 {
00151   // this should call the device destructors, which are blocking until workerthreads return.
00152   devices_.clear ();
00153 
00154   //now its save to free images
00155   for (map<string, ImageContext*>::iterator imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00156   {
00157     delete imageIt->second;
00158   }
00159 
00160   for (map<string, ImageContext*>::iterator imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00161   {
00162     delete imageIt->second;
00163   }
00164 
00165   for (map<string, ImageContext*>::iterator imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00166   {
00167     delete imageIt->second;
00168   }
00169 }
00170 
00171 void MyOpenNIExample::writeImages () const
00172 {
00173   cout << "write images" << endl;
00174   static unsigned index = 0;
00175   ++index;
00176 
00177   cout << "locking rgb images..." << flush;
00178   map<string, ImageContext*>::const_iterator imageIt;
00179   for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00180     imageIt->second->lock.lock ();
00181 
00182   cout << "done\nlocking gray images..." << flush;
00183   for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00184     imageIt->second->lock.lock ();
00185   //  for (imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00186   //    imageIt->second->lock.lock ();
00187   
00188   cout << "locking rgb images..." << flush;
00189   char file_name[255];
00190   for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00191   {
00192     sprintf (file_name, "rgb_%03u.png", index);
00193     imwrite (file_name, imageIt->second->image);
00194   }
00195 
00196   for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00197   {
00198     sprintf (file_name, "gray_%03u.png", index);
00199     imwrite (file_name, imageIt->second->image);
00200   }
00201   //  cout << "write depth images" << endl;
00202   //  for (imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00203   //  {
00204   //    sprintf (file_name, "%s_depth_%03d.png", imageIt->first, index);
00205   //    cout << "saving depth image: " << file_name << endl;
00206   //    imwrite (file_name, imageIt->second->image);
00207   //  }
00208 
00209   for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00210     imageIt->second->lock.unlock ();
00211 
00212   for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00213     imageIt->second->lock.unlock ();
00214   //  for (imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00215   //    imageIt->second->lock.unlock ();
00216 }
00217 
00218 void MyOpenNIExample::imageCallback (boost::shared_ptr<Image> image, void* cookie)
00219 {
00220   timeval timestamp;
00221   gettimeofday (&timestamp, NULL);
00222 
00223   double now = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
00224 //  double diff1 = min (fabs (now - depth_timestamp), fabs (depth_timestamp - image_timestamp));
00225 //  double diff2 = max (fabs (now - depth_timestamp), fabs (depth_timestamp - image_timestamp));
00226   //cout << diff1 * 1000.0 << "\tms vs. " << diff2 * 1000.0 << endl;
00227 
00228   image_timestamp = now;
00229   OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00230   ImageContext* rgb_image_context = rgb_images_[device->getConnectionString ()];
00231   ImageContext* gray_image_context = gray_images_[device->getConnectionString ()];
00232 
00233   // lock image so it does not get drawn
00234   unique_lock<mutex> rgb_lock (rgb_image_context->lock);
00235   unsigned char* rgb_buffer = (unsigned char*)(rgb_image_context->image.data + (rgb_image_context->image.cols >> 2) * rgb_image_context->image.elemSize () +
00236                                               (rgb_image_context->image.rows >> 2) * rgb_image_context->image.step);
00237   image->fillRGB (rgb_image_context->image.cols >> 1, rgb_image_context->image.rows >> 1, rgb_buffer, rgb_image_context->image.step);
00238   
00239 /*
00240   unsigned char* rgb_buffer = (unsigned char*)(rgb_image_context->image.data + (rgb_image_context->image.cols >> 3 ) * 3 * rgb_image_context->image.elemSize () +
00241                                               (rgb_image_context->image.rows >> 3 ) * 3 * rgb_image_context->image.step);
00242   image->fillRGB (rgb_image_context->image.cols >> 2, rgb_image_context->image.rows >> 2, rgb_buffer, rgb_image_context->image.step);
00243 */
00244   //image->fillRGB (rgb_image_context->image.cols, rgb_image_context->image.rows, rgb_image_context->image.data, rgb_image_context->image.step);
00245   /*
00246   cv::Mat raw (image->getHeight(), image->getWidth(), CV_8UC1);
00247   image->fillRaw (raw.data);
00248 
00249   static int calls = 0;
00250   if (++calls % 30)
00251   {
00252     int index = calls / 30;
00253     char filename [1024];
00254     sprintf (filename, "image_%03d.png", index);
00255     imwrite (filename, raw);
00256   }
00257   imshow ("raw", raw);
00258   */
00259   rgb_image_context->is_new = true;
00260   rgb_lock.unlock ();
00261 
00262   unique_lock<mutex> gray_lock (gray_image_context->lock);
00263 
00264   unsigned char* gray_buffer = (unsigned char*)(gray_image_context->image.data + (gray_image_context->image.cols >> 2) +
00265                                                (gray_image_context->image.rows >> 2) * gray_image_context->image.step);
00266   image->fillGrayscale (gray_image_context->image.cols >> 1, gray_image_context->image.rows >> 1, gray_buffer, gray_image_context->image.step);
00267   //image->fillGrayscale (gray_image_context->image.cols, gray_image_context->image.rows, gray_image_context->image.data, gray_image_context->image.step);
00268   gray_image_context->is_new = true;
00269 }
00270 
00271 void MyOpenNIExample::depthCallback (boost::shared_ptr<DepthImage> depth, void* cookie)
00272 {
00273 
00274   timeval timestamp;
00275   gettimeofday (&timestamp, NULL);
00276   depth_timestamp = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
00277 
00278   OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00279   ImageContext* depth_image_context = depth_images_[device->getConnectionString ()];
00280 
00281   // lock depth image so it does not get drawn
00282   unique_lock<mutex> depth_lock (depth_image_context->lock);
00283   float* buffer = (float*)(depth_image_context->image.data + (depth_image_context->image.cols >> 2) * sizeof(float) +
00284                           (depth_image_context->image.rows >> 2) * depth_image_context->image.step );
00285   depth->fillDepthImage (depth_image_context->image.cols >> 1, depth_image_context->image.rows >> 1, buffer, depth_image_context->image.step);
00286   //depth.fillDepthImage (depth_image_context->image.cols, depth_image_context->image.rows, (float*)depth_image_context->image.data, depth_image_context->image.step);
00287   depth_image_context->is_new = true;
00288 }
00289 
00290 int MyOpenNIExample::run ()
00291 {
00292   running_ = true;
00293   try
00294   {
00295     while (running_)
00296     {
00297       for (map<string, ImageContext*>::iterator imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00298       {
00299         if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
00300         {
00301           cv::Mat bgr_image;
00302           cvtColor (imageIt->second->image, bgr_image, CV_RGB2BGR);
00303           imshow (imageIt->first + "RGB", bgr_image);
00304           imageIt->second->is_new = false;
00305           imageIt->second->lock.unlock ();
00306         }
00307       }
00308 
00309       for (map<string, ImageContext*>::iterator imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00310       {
00311         if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
00312         {
00313           imshow (imageIt->first + "Gray", imageIt->second->image);
00314           imageIt->second->is_new = false;
00315           imageIt->second->lock.unlock ();
00316         }
00317       }
00318 
00319       for (map<string, ImageContext*>::iterator imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00320       {
00321         if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
00322         {// depth image is in range 0-10 meter -> convert to 0-255 values
00323           Mat gray_image;
00324           imageIt->second->image.convertTo (gray_image, CV_8UC1, 25.5);
00325           imshow (imageIt->first + "Depth", gray_image);
00326           imageIt->second->is_new = false;
00327           imageIt->second->lock.unlock ();
00328         }
00329       }
00330 
00331       unsigned char key = waitKey (30) & 0xFF;
00332 
00333       switch (key)
00334       {
00335         case 27:
00336         case 'q':
00337         case 'Q': running_ = false;
00338           break;
00339 
00340         case '1':
00341           selected_device_ = 0;
00342           break;
00343         case '2':
00344           selected_device_ = 1;
00345           break;
00346         case '3':
00347           selected_device_ = 2;
00348           break;
00349 
00350         case 'r':
00351         case 'R':
00352           devices_[selected_device_]->setDepthRegistration (!devices_[selected_device_]->isDepthRegistered ());
00353           break;
00354         case 's':
00355         case 'S':
00356           if (devices_[selected_device_]->isSynchronizationSupported ())
00357             devices_[selected_device_]->setSynchronization (!devices_[selected_device_]->isSynchronized ());
00358           break;
00359         case 'c':
00360         case 'C':
00361           if (devices_[selected_device_]->isDepthCropped ())
00362           {
00363             depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.lock ();
00364             //depth_images_[devices_[selected_device_]->getConnectionString ()]->image.create (480, 640, CV_32FC1);
00365             depth_images_[devices_[selected_device_]->getConnectionString ()]->image.rows = 480;
00366             depth_images_[devices_[selected_device_]->getConnectionString ()]->image.cols = 640;
00367             depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.unlock ();
00368             devices_[selected_device_]->setDepthCropping (0, 0, 0, 0);
00369           }
00370           else if (devices_[selected_device_]->isDepthCroppingSupported ())
00371           {
00372             depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.lock ();
00373             //depth_images_[devices_[selected_device_]->getConnectionString ()]->image.create (300, 400, CV_32FC1);
00374             depth_images_[devices_[selected_device_]->getConnectionString ()]->image.rows = 240;
00375             depth_images_[devices_[selected_device_]->getConnectionString ()]->image.cols = 320;
00376             depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.unlock ();
00377             devices_[selected_device_]->setDepthCropping (100, 100, 320, 240);
00378           }
00379           break;
00380 
00381         case 'd':
00382         case 'D':
00383           if (devices_[selected_device_]->isDepthStreamRunning ())
00384             devices_[selected_device_]->stopDepthStream ();
00385           else
00386             devices_[selected_device_]->startDepthStream ();
00387           break;
00388         case 'i':
00389         case 'I':
00390           if (devices_[selected_device_]->isImageStreamRunning ())
00391             devices_[selected_device_]->stopImageStream ();
00392           else
00393             devices_[selected_device_]->startImageStream ();
00394           break;
00395 
00396         case 'w':
00397         case 'W':
00398           writeImages ();
00399           break;
00400       }
00401     }
00402   }
00403   catch (const OpenNIException& exception)
00404   {
00405     cout << "exception caught: " << exception.what () << endl;
00406     return (-1);
00407   }
00408   catch (...)
00409   {
00410 
00411     cout << "unknown exception caught" << endl;
00412     return (-1);
00413   }
00414   return 0;
00415 }
00416 
00417 int main (int argc, char** argv)
00418 {
00419   OpenNIDriver& driver = OpenNIDriver::getInstance ();
00420   if (argc == 1)
00421   {
00422     cout << "Usage: " << argv[0] << " (<device-index>)+" << endl;
00423     if (driver.getNumberDevices () > 0)
00424     {
00425       for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00426       {
00427         cout << "Device: " << deviceIdx << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
00428                 << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00429       }
00430     }
00431     else
00432       cout << "No devices connected." << endl;
00433     exit (1);
00434   }
00435 
00436   vector <unsigned> device_indices;
00437   for (int argIdx = 1; argIdx < argc; ++argIdx)
00438   {
00439     unsigned deviceIdx = (unsigned)atoi (argv[argIdx]);
00440     if (deviceIdx >= driver.getNumberDevices ())
00441     {
00442       if (driver.getNumberDevices () > 0)
00443       {
00444         cout << "Device index out of range. " << driver.getNumberDevices () << " devices found." << endl;
00445         for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00446         {
00447           cout << "Device: " << deviceIdx << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: "
00448                   << driver.getProductName (deviceIdx) << ", connected: " << (int)driver.getBus (deviceIdx) << " @ "
00449                   << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00450         }
00451       }
00452       else
00453         cout << "No devices connected." << endl;
00454       exit (-1);
00455     }
00456     device_indices.push_back ((unsigned)deviceIdx);
00457   }
00458 
00459   cout << "<1,2,3...> to select device" << endl;
00460   cout << "<I> to start or stop image stream of selected device" << endl;
00461   cout << "<D> to start or stop depth stream of selected device" << endl;
00462   cout << "<R> to turn on or off registration for selected device" << endl;
00463   cout << "<S> to turn on or off synchronization for selected device" << endl;
00464   cout << "<C> to turn on or off image cropping for selected device" << endl;
00465   cout << "<W> write current images" << endl;
00466   cout << "<Q> to quit application" << endl;
00467   MyOpenNIExample example (device_indices);
00468 
00469   return example.run ();
00470 }


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Mon Oct 6 2014 03:06:43