openni_device.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 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_depth_image.h>
00040 #include <openni_camera/openni_ir_image.h>
00041 #include <iostream>
00042 #include <limits>
00043 #include <sstream>
00044 #include <map>
00045 #include <vector>
00046 
00047 
00048 using namespace std;
00049 using namespace boost;
00050 
00051 namespace openni_wrapper
00052 {
00053 
00054 OpenNIDevice::OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node) throw (OpenNIException)
00055   : context_ (context)
00056   , device_node_info_ (device_node)
00057 {
00058   // create the production nodes
00059   XnStatus status = context_.CreateProductionTree (const_cast<xn::NodeInfo&>(depth_node));
00060   if (status != XN_STATUS_OK)
00061     THROW_OPENNI_EXCEPTION ("creating depth generator failed. Reason: %s", xnGetStatusString (status));
00062 
00063   status = context_.CreateProductionTree (const_cast<xn::NodeInfo&>(image_node));
00064   if (status != XN_STATUS_OK)
00065     THROW_OPENNI_EXCEPTION ("creating image generator failed. Reason: %s", xnGetStatusString (status));
00066 
00067   status = context_.CreateProductionTree (const_cast<xn::NodeInfo&>(ir_node));
00068   if (status != XN_STATUS_OK)
00069     THROW_OPENNI_EXCEPTION ("creating IR generator failed. Reason: %s", xnGetStatusString (status));
00070 
00071   // get production node instances
00072   status = depth_node.GetInstance (depth_generator_);
00073   if (status != XN_STATUS_OK)
00074     THROW_OPENNI_EXCEPTION ("creating depth generator instance failed. Reason: %s", xnGetStatusString (status));
00075 
00076   status = image_node.GetInstance (image_generator_);
00077   if (status != XN_STATUS_OK)
00078     THROW_OPENNI_EXCEPTION ("creating image generator instance failed. Reason: %s", xnGetStatusString (status));
00079 
00080   status = ir_node.GetInstance (ir_generator_);
00081   if (status != XN_STATUS_OK)
00082     THROW_OPENNI_EXCEPTION ("creating IR generator instance failed. Reason: %s", xnGetStatusString (status));
00083 
00084   ir_generator_.RegisterToNewDataAvailable ((xn::StateChangedHandler)NewIRDataAvailable, this, ir_callback_handle_);
00085 
00086   depth_generator_.RegisterToNewDataAvailable ((xn::StateChangedHandler)NewDepthDataAvailable, this, depth_callback_handle_);
00087   image_generator_.RegisterToNewDataAvailable ((xn::StateChangedHandler)NewImageDataAvailable, this, image_callback_handle_);
00088 
00089   Init ();
00090 }
00091 
00092 OpenNIDevice::OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node) throw (OpenNIException)
00093   : context_ (context)
00094   , device_node_info_ (device_node)
00095 {
00096     // create the production nodes
00097   XnStatus status = context_.CreateProductionTree (const_cast<xn::NodeInfo&>(depth_node));
00098   if (status != XN_STATUS_OK)
00099     THROW_OPENNI_EXCEPTION ("creating depth generator failed. Reason: %s", xnGetStatusString (status));
00100 
00101   status = context_.CreateProductionTree (const_cast<xn::NodeInfo&>(ir_node));
00102   if (status != XN_STATUS_OK)
00103     THROW_OPENNI_EXCEPTION ("creating IR generator failed. Reason: %s", xnGetStatusString (status));
00104 
00105   // get production node instances
00106   status = depth_node.GetInstance (depth_generator_);
00107   if (status != XN_STATUS_OK)
00108     THROW_OPENNI_EXCEPTION ("creating depth generator instance failed. Reason: %s", xnGetStatusString (status));
00109 
00110   status = ir_node.GetInstance (ir_generator_);
00111   if (status != XN_STATUS_OK)
00112     THROW_OPENNI_EXCEPTION ("creating IR generator instance failed. Reason: %s", xnGetStatusString (status));
00113   
00114   ir_generator_.RegisterToNewDataAvailable ((xn::StateChangedHandler)NewIRDataAvailable, this, ir_callback_handle_);
00115 
00116   depth_generator_.RegisterToNewDataAvailable ((xn::StateChangedHandler)NewDepthDataAvailable, this, depth_callback_handle_);
00117   // set up rest
00118   Init ();  
00119 }
00120 
00121 // For ONI Player devices
00122 OpenNIDevice::OpenNIDevice (xn::Context& context) throw (OpenNIException)
00123   : context_ (context)
00124   , device_node_info_ (0)
00125 {
00126 }
00127 
00128 OpenNIDevice::~OpenNIDevice () throw ()
00129 {
00130   shutdown ();
00131 }
00132 
00133 void OpenNIDevice::shutdown ()
00134 {
00135   {
00136     // Lock out all the XxxDataThreadFunction threads
00137     boost::lock_guard<boost::mutex> image_guard(image_mutex_);
00138     boost::lock_guard<boost::mutex> depth_guard(depth_mutex_);
00139     boost::lock_guard<boost::mutex> ir_guard(ir_mutex_);
00140     
00141     // Stop streams
00142     if (image_generator_.IsValid() && image_generator_.IsGenerating ())
00143       image_generator_.StopGenerating ();
00144 
00145     if (depth_generator_.IsValid () && depth_generator_.IsGenerating ())
00146       depth_generator_.StopGenerating ();
00147 
00148     if (ir_generator_.IsValid () && ir_generator_.IsGenerating ())
00149       ir_generator_.StopGenerating ();
00150 
00151     // On wakeup, each data thread will check quit_ and exit
00152     quit_ = true;
00153   }
00154 
00155   // Wake up, time to die
00156   depth_condition_.notify_all ();
00157   image_condition_.notify_all ();
00158   ir_condition_.notify_all ();
00159   data_threads_.join_all ();
00160 }
00161 
00162 void OpenNIDevice::Init () throw (OpenNIException)
00163 {
00164   quit_ = false;
00165   XnDouble pixel_size;
00166 
00167   // set Depth resolution here only once... since no other mode for kinect is available -> deactivating setDepthResolution method!
00168   if (hasDepthStream ())
00169   {
00170     unique_lock<mutex> depth_lock (depth_mutex_);
00171     XnStatus status = depth_generator_.GetRealProperty ("ZPPS", pixel_size);
00172     if (status != XN_STATUS_OK)
00173       THROW_OPENNI_EXCEPTION ("reading the pixel size of IR camera failed. Reason: %s", xnGetStatusString (status));
00174 
00175     XnUInt64 depth_focal_length_SXGA;
00176     status = depth_generator_.GetIntProperty ("ZPD", depth_focal_length_SXGA);
00177     if (status != XN_STATUS_OK)
00178       THROW_OPENNI_EXCEPTION ("reading the focal length of IR camera failed. Reason: %s", xnGetStatusString (status));
00179 
00180     XnDouble baseline;
00181     status = depth_generator_.GetRealProperty ("LDDIS", baseline);
00182     if (status != XN_STATUS_OK)
00183       THROW_OPENNI_EXCEPTION ("reading the baseline failed. Reason: %s", xnGetStatusString (status));
00184 
00185     status = depth_generator_.GetIntProperty ("ShadowValue", shadow_value_);
00186     if (status != XN_STATUS_OK)
00187       THROW_OPENNI_EXCEPTION ("reading the value for pixels in shadow regions failed. Reason: %s", xnGetStatusString (status));
00188 
00189     status = depth_generator_.GetIntProperty ("NoSampleValue", no_sample_value_);
00190     if (status != XN_STATUS_OK)
00191       THROW_OPENNI_EXCEPTION ("reading the value for pixels with no depth estimation failed. Reason: %s", xnGetStatusString (status));
00192 
00193     // baseline from cm -> meters
00194     baseline_ = (float)(baseline * 0.01);
00195 
00196     //focal length from mm -> pixels (valid for 1280x1024)
00197     depth_focal_length_SXGA_ = (float)depth_focal_length_SXGA / pixel_size;
00198 
00199     data_threads_.create_thread (boost::bind (&OpenNIDevice::DepthDataThreadFunction, this));
00200   }
00201 
00202   if (hasImageStream ())
00203   {
00204     lock_guard<mutex> image_lock (image_mutex_);
00205     data_threads_.create_thread (boost::bind (&OpenNIDevice::ImageDataThreadFunction, this));
00206   }
00207 
00208   if (hasIRStream ())
00209   {
00210     lock_guard<mutex> ir_lock (ir_mutex_);
00211     data_threads_.create_thread (boost::bind (&OpenNIDevice::IRDataThreadFunction, this));
00212   }
00213 }
00214 
00215 void OpenNIDevice::startImageStream () throw (OpenNIException)
00216 {
00217   if (hasImageStream ())
00218   {
00219     lock_guard<mutex> image_lock (image_mutex_);
00220     if (!image_generator_.IsGenerating ())
00221     {
00222       XnStatus status = image_generator_.StartGenerating ();
00223       if (status != XN_STATUS_OK)
00224         THROW_OPENNI_EXCEPTION ("starting image stream failed. Reason: %s", xnGetStatusString (status));
00225     }
00226   }
00227   else
00228     THROW_OPENNI_EXCEPTION ("Device does not provide an image stream");
00229 }
00230 
00231 void OpenNIDevice::stopImageStream () throw (OpenNIException)
00232 {
00233   if (hasImageStream ())
00234   {
00235     lock_guard<mutex> image_lock (image_mutex_);
00236     if (image_generator_.IsGenerating ())
00237     {
00238       XnStatus status = image_generator_.StopGenerating ();
00239       if (status != XN_STATUS_OK)
00240         THROW_OPENNI_EXCEPTION ("stopping image stream failed. Reason: %s", xnGetStatusString (status));
00241     }
00242   }
00243   else
00244     THROW_OPENNI_EXCEPTION ("Device does not provide an image stream");
00245 }
00246 
00247 void OpenNIDevice::startDepthStream () throw (OpenNIException)
00248 {
00249   if (hasDepthStream ())
00250   {
00251     lock_guard<mutex> depth_lock (depth_mutex_);
00252     if (!depth_generator_.IsGenerating ())
00253     {
00254       XnStatus status = depth_generator_.StartGenerating ();
00255 
00256       if (status != XN_STATUS_OK)
00257         THROW_OPENNI_EXCEPTION ("starting depth stream failed. Reason: %s", xnGetStatusString (status));
00258     }
00259   }
00260   else
00261     THROW_OPENNI_EXCEPTION ("Device does not provide a depth stream");
00262 }
00263 
00264 void OpenNIDevice::stopDepthStream () throw (OpenNIException)
00265 {
00266   if (hasDepthStream ())
00267   {
00268     lock_guard<mutex> depth_lock (depth_mutex_);
00269     if (depth_generator_.IsGenerating ())
00270     {
00271       XnStatus status = depth_generator_.StopGenerating ();
00272 
00273       if (status != XN_STATUS_OK)
00274         THROW_OPENNI_EXCEPTION ("stopping depth stream failed. Reason: %s", xnGetStatusString (status));
00275     }
00276   }
00277   else
00278     THROW_OPENNI_EXCEPTION ("Device does not provide a depth stream");
00279 }
00280 
00281 void OpenNIDevice::startIRStream () throw (OpenNIException)
00282 {
00283   if (hasIRStream ())
00284   {
00285     lock_guard<mutex> ir_lock (ir_mutex_);
00286     if (!ir_generator_.IsGenerating ())
00287     {
00288       XnStatus status = ir_generator_.StartGenerating ();
00289 
00290       if (status != XN_STATUS_OK)
00291         THROW_OPENNI_EXCEPTION ("starting IR stream failed. Reason: %s", xnGetStatusString (status));
00292     }
00293   }
00294   else
00295     THROW_OPENNI_EXCEPTION ("Device does not provide an IR stream");
00296 }
00297 
00298 void OpenNIDevice::stopIRStream () throw (OpenNIException)
00299 {
00300   if (hasIRStream ())
00301   {
00302     lock_guard<mutex> ir_lock (ir_mutex_);
00303     if (ir_generator_.IsGenerating ())
00304     {
00305       XnStatus status = ir_generator_.StopGenerating ();
00306 
00307       if (status != XN_STATUS_OK)
00308         THROW_OPENNI_EXCEPTION ("stopping IR stream failed. Reason: %s", xnGetStatusString (status));
00309     }
00310   }
00311   else
00312     THROW_OPENNI_EXCEPTION ("Device does not provide an IR stream");
00313 }
00314 
00315 bool OpenNIDevice::isImageStreamRunning () const throw (OpenNIException)
00316 {
00317   lock_guard<mutex> image_lock (image_mutex_);
00318   return ( image_generator_.IsValid () && image_generator_.IsGenerating ());
00319 }
00320 
00321 bool OpenNIDevice::isDepthStreamRunning () const throw (OpenNIException)
00322 {
00323   lock_guard<mutex> depth_lock (depth_mutex_);
00324   return ( depth_generator_.IsValid () && depth_generator_.IsGenerating ());
00325 }
00326 
00327 bool OpenNIDevice::isIRStreamRunning () const throw (OpenNIException)
00328 {
00329   lock_guard<mutex> ir_lock (ir_mutex_);
00330   return ( ir_generator_.IsValid () && ir_generator_.IsGenerating ());
00331 }
00332 
00333 bool OpenNIDevice::hasImageStream () const throw ()
00334 {
00335   lock_guard<mutex> lock (image_mutex_);
00336   return image_generator_.IsValid ();
00337   //return (available_image_modes_.size() != 0);
00338 }
00339 
00340 bool OpenNIDevice::hasDepthStream () const throw ()
00341 {
00342   lock_guard<mutex> lock (depth_mutex_);
00343   return depth_generator_.IsValid ();
00344   //return (available_depth_modes_.size() != 0);
00345 }
00346 
00347 bool OpenNIDevice::hasIRStream () const throw ()
00348 {
00349   lock_guard<mutex> ir_lock (ir_mutex_);
00350   return ir_generator_.IsValid ();
00351 }
00352 
00353 void OpenNIDevice::setDepthRegistration (bool on_off) throw (OpenNIException)
00354 {
00355   if (hasDepthStream () && hasImageStream())
00356   {
00357     lock_guard<mutex> image_lock (image_mutex_);
00358     lock_guard<mutex> depth_lock (depth_mutex_);
00359     if (on_off && !depth_generator_.GetAlternativeViewPointCap ().IsViewPointAs (image_generator_))
00360     {
00361       if (depth_generator_.GetAlternativeViewPointCap ().IsViewPointSupported (image_generator_))
00362       {
00363         XnStatus status = depth_generator_.GetAlternativeViewPointCap ().SetViewPoint (image_generator_);
00364         if (status != XN_STATUS_OK)
00365           THROW_OPENNI_EXCEPTION ("turning registration on failed. Reason: %s", xnGetStatusString (status));
00366       }
00367       else
00368         THROW_OPENNI_EXCEPTION ("turning registration on failed. Reason: unsopported viewpoint");
00369     }
00370     else if (!on_off)
00371     {
00372       XnStatus status = depth_generator_.GetAlternativeViewPointCap ().ResetViewPoint ();
00373 
00374       if (status != XN_STATUS_OK)
00375         THROW_OPENNI_EXCEPTION ("turning registration off failed. Reason: %s", xnGetStatusString (status));
00376     }
00377   }
00378   else
00379     THROW_OPENNI_EXCEPTION ("Device does not provide image + depth stream");
00380 }
00381 
00382 bool OpenNIDevice::isDepthRegistered () const throw (OpenNIException)
00383 {
00384   if (hasDepthStream () && hasImageStream() )
00385   {
00386     xn::DepthGenerator& depth_generator = const_cast<xn::DepthGenerator&>(depth_generator_);
00387     xn::ImageGenerator& image_generator = const_cast<xn::ImageGenerator&>(image_generator_);
00388 
00389     lock_guard<mutex> image_lock (image_mutex_);
00390     lock_guard<mutex> depth_lock (depth_mutex_);
00391     return (depth_generator.GetAlternativeViewPointCap ().IsViewPointAs (image_generator));
00392   }
00393   return false;
00394 }
00395 
00396 bool OpenNIDevice::isDepthRegistrationSupported () const throw (OpenNIException)
00397 {
00398   lock_guard<mutex> image_lock (image_mutex_);
00399   lock_guard<mutex> depth_lock (depth_mutex_);
00400   xn::ImageGenerator& image_generator = const_cast<xn::ImageGenerator&> (image_generator_);
00401   return ( depth_generator_.IsValid() && image_generator_.IsValid() && depth_generator_.GetAlternativeViewPointCap().IsViewPointSupported(image_generator));
00402 }
00403 
00404 bool OpenNIDevice::isSynchronizationSupported () const throw ()
00405 {
00406   lock_guard<mutex> image_lock (image_mutex_);
00407   lock_guard<mutex> depth_lock (depth_mutex_);
00408   return ( depth_generator_.IsValid() && image_generator_.IsValid() && depth_generator_.IsCapabilitySupported (XN_CAPABILITY_FRAME_SYNC));
00409 }
00410 
00411 void OpenNIDevice::setSynchronization (bool on_off) throw (OpenNIException)
00412 {
00413   if (hasDepthStream () && hasImageStream())
00414   {
00415     lock_guard<mutex> image_lock (image_mutex_);
00416     lock_guard<mutex> depth_lock (depth_mutex_);
00417     XnStatus status;
00418 
00419     if (on_off && !depth_generator_.GetFrameSyncCap ().IsFrameSyncedWith (image_generator_))
00420     {
00421       status = depth_generator_.GetFrameSyncCap ().FrameSyncWith (image_generator_);
00422       if (status != XN_STATUS_OK)
00423         THROW_OPENNI_EXCEPTION ("could not turn on frame synchronization. Reason: %s", xnGetStatusString (status));
00424     }
00425     else if (!on_off && depth_generator_.GetFrameSyncCap ().IsFrameSyncedWith (image_generator_))
00426     {
00427       status = depth_generator_.GetFrameSyncCap ().StopFrameSyncWith (image_generator_);
00428       if (status != XN_STATUS_OK)
00429         THROW_OPENNI_EXCEPTION ("could not turn off frame synchronization. Reason: %s", xnGetStatusString (status));
00430     }
00431   }
00432   else
00433     THROW_OPENNI_EXCEPTION ("Device does not provide image + depth stream");
00434 }
00435 
00436 bool OpenNIDevice::isSynchronized () const throw (OpenNIException)
00437 {
00438   if (hasDepthStream () && hasImageStream())
00439   {
00440     lock_guard<mutex> image_lock (image_mutex_);
00441     lock_guard<mutex> depth_lock (depth_mutex_);
00442     xn::DepthGenerator& depth_generator = const_cast<xn::DepthGenerator&>(depth_generator_);
00443     xn::ImageGenerator& image_generator = const_cast<xn::ImageGenerator&>(image_generator_);
00444     return (depth_generator.GetFrameSyncCap ().CanFrameSyncWith (image_generator) && depth_generator.GetFrameSyncCap ().IsFrameSyncedWith (image_generator));
00445   }
00446   else
00447     return false;
00448 }
00449 
00450 bool OpenNIDevice::isDepthCroppingSupported () const throw ()
00451 {
00452   lock_guard<mutex> depth_lock (depth_mutex_);
00453   return ( image_generator_.IsValid() && depth_generator_.IsCapabilitySupported (XN_CAPABILITY_CROPPING) );
00454 }
00455 
00456 bool OpenNIDevice::isDepthCropped () const throw (OpenNIException)
00457 {
00458   if (hasDepthStream ())
00459   {
00460     lock_guard<mutex> depth_lock (depth_mutex_);
00461     XnCropping cropping;
00462     xn::DepthGenerator& depth_generator = const_cast<xn::DepthGenerator&>(depth_generator_);
00463     XnStatus status = depth_generator.GetCroppingCap ().GetCropping (cropping);
00464     if (status != XN_STATUS_OK)
00465       THROW_OPENNI_EXCEPTION ("could not read cropping information for depth stream. Reason: %s", xnGetStatusString (status));
00466 
00467     return cropping.bEnabled;
00468   }
00469   return false;
00470 }
00471 
00472 void OpenNIDevice::setDepthCropping (unsigned x, unsigned y, unsigned width, unsigned height) throw (OpenNIException)
00473 {
00474   if (hasDepthStream ())
00475   {
00476     lock_guard<mutex> depth_lock (depth_mutex_);
00477     XnCropping cropping;
00478     cropping.nXOffset = x;
00479     cropping.nYOffset = y;
00480     cropping.nXSize   = width;
00481     cropping.nYSize   = height;
00482 
00483     cropping.bEnabled = (width != 0 && height != 0);
00484     XnStatus status = depth_generator_.GetCroppingCap ().SetCropping (cropping);
00485     if (status != XN_STATUS_OK)
00486       THROW_OPENNI_EXCEPTION ("could not set cropping information for depth stream. Reason: %s", xnGetStatusString (status));
00487   }
00488   else
00489     THROW_OPENNI_EXCEPTION ("Device does not provide depth stream");
00490 }
00491 
00492 void OpenNIDevice::ImageDataThreadFunction () throw (OpenNIException)
00493 {
00494   while (true)
00495   {
00496     // lock before checking running flag
00497     unique_lock<mutex> image_lock (image_mutex_);
00498     if (quit_)
00499       return;
00500     image_condition_.wait (image_lock);
00501     if (quit_)
00502       return;
00503 
00504     image_generator_.WaitAndUpdateData ();
00505     xn::ImageMetaData image_md;
00506     image_generator_.GetMetaData (image_md);
00507     boost::shared_ptr<xn::ImageMetaData> image_data (new xn::ImageMetaData);
00508     XnStatus xs = image_data->CopyFrom (image_md);
00509     image_lock.unlock ();
00510     
00511     if (xs != XN_STATUS_OK)
00512        continue;
00513 
00514     boost::shared_ptr<Image> image = getCurrentImage (image_data);
00515     for (map< OpenNIDevice::CallbackHandle, ActualImageCallbackFunction >::iterator callbackIt = image_callback_.begin (); callbackIt != image_callback_.end (); ++callbackIt)
00516     {
00517       callbackIt->second.operator()(image);
00518     }
00519   }
00520 }
00521 
00522 void OpenNIDevice::DepthDataThreadFunction () throw (OpenNIException)
00523 {
00524   while (true)
00525   {
00526     // lock before checking running flag
00527     unique_lock<mutex> depth_lock (depth_mutex_);
00528     if (quit_)
00529       return;
00530     depth_condition_.wait (depth_lock);
00531     if (quit_)
00532       return;
00533 
00534     depth_generator_.WaitAndUpdateData ();
00535     xn::DepthMetaData image_md;
00536     depth_generator_.GetMetaData (image_md);
00537     boost::shared_ptr<xn::DepthMetaData> depth_data (new xn::DepthMetaData);
00538     XnStatus xs = depth_data->CopyFrom (image_md);
00539     depth_lock.unlock ();
00540     
00541     if (xs != XN_STATUS_OK)
00542        continue;
00543 
00544     boost::shared_ptr<DepthImage> depth_image ( new DepthImage (depth_data, baseline_, getDepthFocalLength (), shadow_value_, no_sample_value_) );
00545 
00546     for (map< OpenNIDevice::CallbackHandle, ActualDepthImageCallbackFunction >::iterator callbackIt = depth_callback_.begin ();
00547          callbackIt != depth_callback_.end (); ++callbackIt)
00548     {
00549       callbackIt->second.operator()(depth_image);
00550     }
00551   }
00552 }
00553 
00554 void OpenNIDevice::IRDataThreadFunction () throw (OpenNIException)
00555 {
00556   while (true)
00557   {
00558     // lock before checking running flag
00559     unique_lock<mutex> ir_lock (ir_mutex_);
00560     if (quit_)
00561       return;
00562     ir_condition_.wait (ir_lock);
00563     if (quit_)
00564       return;
00565 
00566     ir_generator_.WaitAndUpdateData ();
00567     xn::IRMetaData image_md;
00568     ir_generator_.GetMetaData (image_md);
00569     boost::shared_ptr<xn::IRMetaData> ir_data (new xn::IRMetaData);
00570     XnStatus xs = ir_data->CopyFrom (image_md);
00571     ir_lock.unlock ();
00572 
00573     if (xs != XN_STATUS_OK)
00574        continue;
00575 
00576     boost::shared_ptr<IRImage> ir_image ( new IRImage (ir_data) );
00577 
00578     for (map< OpenNIDevice::CallbackHandle, ActualIRImageCallbackFunction >::iterator callbackIt = ir_callback_.begin ();
00579          callbackIt != ir_callback_.end (); ++callbackIt)
00580     {
00581       callbackIt->second.operator()(ir_image);
00582     }
00583   }
00584 }
00585 
00586 void __stdcall OpenNIDevice::NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw ()
00587 {
00588   OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00589   device->depth_condition_.notify_all ();
00590 }
00591 
00592 void __stdcall OpenNIDevice::NewImageDataAvailable (xn::ProductionNode& node, void* cookie) throw ()
00593 {
00594   OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00595   device->image_condition_.notify_all ();
00596 }
00597 
00598 void __stdcall OpenNIDevice::NewIRDataAvailable (xn::ProductionNode& node, void* cookie) throw ()
00599 {
00600   OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00601   device->ir_condition_.notify_all ();
00602 }
00603 
00604 OpenNIDevice::CallbackHandle OpenNIDevice::registerImageCallback (const ImageCallbackFunction& callback, void* custom_data) throw ()
00605 {
00606   if (!hasImageStream ())
00607     THROW_OPENNI_EXCEPTION ("Device does not provide an image stream");
00608 
00609   image_callback_[image_callback_handle_counter_] = boost::bind (callback, _1, custom_data);
00610   return image_callback_handle_counter_++;
00611 }
00612 
00613 bool OpenNIDevice::unregisterImageCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw ()
00614 {
00615   if (!hasImageStream ())
00616     THROW_OPENNI_EXCEPTION ("Device does not provide an image stream");
00617 
00618   return (image_callback_.erase (callbackHandle) != 0);
00619 }
00620 
00621 OpenNIDevice::CallbackHandle OpenNIDevice::registerDepthCallback (const DepthImageCallbackFunction& callback, void* custom_data) throw ()
00622 {
00623   if (!hasDepthStream ())
00624     THROW_OPENNI_EXCEPTION ("Device does not provide a depth image");
00625 
00626   depth_callback_[depth_callback_handle_counter_] = boost::bind (callback, _1, custom_data);
00627   return depth_callback_handle_counter_++;
00628 }
00629 
00630 bool OpenNIDevice::unregisterDepthCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw ()
00631 {
00632   if (hasDepthStream ())
00633     return (depth_callback_.erase (callbackHandle) != 0);
00634   else
00635     THROW_OPENNI_EXCEPTION ("Device does not provide a depth image");
00636   return false;
00637 }
00638 
00639 OpenNIDevice::CallbackHandle OpenNIDevice::registerIRCallback (const IRImageCallbackFunction& callback, void* custom_data) throw ()
00640 {
00641   if (!hasDepthStream ())
00642     THROW_OPENNI_EXCEPTION ("Device does not provide an IR stream");
00643 
00644   ir_callback_[ir_callback_handle_counter_] = boost::bind (callback, _1, custom_data);
00645   return ir_callback_handle_counter_++;
00646 }
00647 
00648 bool OpenNIDevice::unregisterIRCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw ()
00649 {
00650   if (!hasDepthStream ())
00651     THROW_OPENNI_EXCEPTION ("Device does not provide an IR stream");
00652 
00653   return (ir_callback_.erase (callbackHandle) != 0);
00654 }
00655 
00656 const char* OpenNIDevice::getSerialNumber () throw ()
00657 {
00658     const char* openni_serial = device_node_info_.GetInstanceName ();
00659     if (strlen(openni_serial)>0 && strcmp(openni_serial, "Device1")) {
00660         return openni_serial;
00661     } else {
00662         char *primesense_serial = (char*)malloc(XN_MAX_NAME_LENGTH); // memleak
00663         context_.CreateProductionTree(device_node_info_);
00664         xn::Device device;
00665 
00666         if(device_node_info_.GetInstance(device) != XN_STATUS_OK) {
00667             THROW_OPENNI_EXCEPTION ("couldn't get device instance for reading serial no.");
00668         }
00669 
00670         xn::DeviceIdentificationCapability d = device.GetIdentificationCap();
00671 
00672         d.GetSerialNumber(primesense_serial,XN_MAX_NAME_LENGTH);
00673 
00674         device.Release();
00675         return primesense_serial;
00676     }
00677 }
00678 
00679 const char* OpenNIDevice::getConnectionString () const throw ()
00680 {
00681   return device_node_info_.GetCreationInfo ();
00682 }
00683 
00684 unsigned short OpenNIDevice::getVendorID () const throw ()
00685 {
00686   unsigned short vendor_id;
00687   unsigned short product_id;
00688   
00689 #ifndef _WIN32
00690   unsigned char bus;
00691   unsigned char address;
00692   
00693   sscanf (device_node_info_.GetCreationInfo(), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00694 
00695 #else
00696   OpenNIDriver::getDeviceType (device_node_info_.GetCreationInfo(), vendor_id, product_id);
00697 #endif
00698   return vendor_id;
00699 }
00700 
00701 unsigned short OpenNIDevice::getProductID () const throw ()
00702 {
00703   unsigned short vendor_id;
00704   unsigned short product_id;
00705 #ifndef _WIN32
00706   unsigned char bus;
00707   unsigned char address;
00708   sscanf (device_node_info_.GetCreationInfo(), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00709 
00710 #else
00711   OpenNIDriver::getDeviceType (device_node_info_.GetCreationInfo(), vendor_id, product_id);
00712 #endif
00713   return product_id;
00714 }
00715 
00716 unsigned char OpenNIDevice::getBus () const throw ()
00717 {
00718   unsigned char bus = 0;
00719 #ifndef _WIN32
00720   unsigned short vendor_id;
00721   unsigned short product_id;
00722   unsigned char address;
00723   sscanf (device_node_info_.GetCreationInfo(), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00724 #endif
00725   return bus;
00726 }
00727 
00728 unsigned char OpenNIDevice::getAddress () const throw ()
00729 {
00730   unsigned char address = 0;
00731 #ifndef _WIN32
00732   unsigned short vendor_id;
00733   unsigned short product_id;
00734   unsigned char bus;
00735   sscanf (device_node_info_.GetCreationInfo(), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00736 #endif
00737   return address;
00738 }
00739 
00740 const char* OpenNIDevice::getVendorName () const throw ()
00741 {
00742   XnProductionNodeDescription& description = const_cast<XnProductionNodeDescription&>(device_node_info_.GetDescription ());
00743   return description.strVendor;
00744 }
00745 
00746 const char* OpenNIDevice::getProductName () const throw ()
00747 {
00748   XnProductionNodeDescription& description = const_cast<XnProductionNodeDescription&>(device_node_info_.GetDescription ());
00749   return description.strName;
00750 }
00751 
00752 bool OpenNIDevice::findCompatibleImageMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode) const throw (OpenNIException)
00753 {
00754   if (isImageModeSupported (output_mode))
00755   {
00756     mode = output_mode;
00757     return true;
00758   }
00759   else
00760   {
00761     bool found = false;
00762     for (vector<XnMapOutputMode>::const_iterator modeIt = available_image_modes_.begin (); modeIt != available_image_modes_.end (); ++modeIt)
00763     {
00764       if (modeIt->nFPS == output_mode.nFPS && isImageResizeSupported (modeIt->nXRes, modeIt->nYRes, output_mode.nXRes, output_mode.nYRes))
00765       {
00766         if (found)
00767         { // check wheter the new mode is better -> smaller than the current one.
00768           if (mode.nXRes * mode.nYRes > modeIt->nXRes * modeIt->nYRes )
00769             mode = *modeIt;
00770         }
00771         else
00772         {
00773           mode = *modeIt;
00774           found = true;
00775         }
00776       }
00777     }
00778     return found;
00779   }
00780 }
00781 
00782 bool OpenNIDevice::findCompatibleDepthMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode) const throw (OpenNIException)
00783 {
00784   if (isDepthModeSupported (output_mode))
00785   {
00786     mode = output_mode;
00787     return true;
00788   }
00789   else
00790   {
00791     bool found = false;
00792     for (vector<XnMapOutputMode>::const_iterator modeIt = available_depth_modes_.begin (); modeIt != available_depth_modes_.end (); ++modeIt)
00793     {
00794       if (modeIt->nFPS == output_mode.nFPS && isImageResizeSupported (modeIt->nXRes, modeIt->nYRes, output_mode.nXRes, output_mode.nYRes))
00795       {
00796         if (found)
00797         { // check wheter the new mode is better -> smaller than the current one.
00798           if (mode.nXRes * mode.nYRes > modeIt->nXRes * modeIt->nYRes )
00799             mode = *modeIt;
00800         }
00801         else
00802         {
00803           mode = *modeIt;
00804           found = true;
00805         }
00806       }
00807     }
00808     return found;
00809   }
00810 }
00811 
00812 void OpenNIDevice::enumAvailableModes () throw (OpenNIException)
00813 {
00814   // we use the overloaded methods from subclasses!
00815   /*
00816   available_image_modes_.clear ();
00817   unique_lock<mutex> image_lock (image_mutex_);
00818   unsigned mode_count = image_generator_.GetSupportedMapOutputModesCount ();
00819   XnMapOutputMode* modes = new XnMapOutputMode[mode_count];
00820   XnStatus status = image_generator_.GetSupportedMapOutputModes (modes, mode_count);
00821   if (status != XN_STATUS_OK)
00822   {
00823     delete[] modes;
00824     THROW_OPENNI_EXCEPTION ("Could not enumerate image stream output modes. Reason: %s", xnGetStatusString (status));
00825   }
00826   image_lock.unlock ();
00827 
00828   for (unsigned modeIdx = 0; modeIdx < mode_count; ++modeIdx)
00829     available_image_modes_.push_back (modes[modeIdx]);
00830   delete[] modes;
00831 
00832   available_depth_modes_.clear ();
00833   unique_lock<mutex> depth_lock (depth_mutex_);
00834   mode_count = depth_generator_.GetSupportedMapOutputModesCount ();
00835   modes = new XnMapOutputMode[mode_count];
00836   status = depth_generator_.GetSupportedMapOutputModes (modes, mode_count);
00837   if (status != XN_STATUS_OK)
00838   {
00839     delete[] modes;
00840     THROW_OPENNI_EXCEPTION ("Could not enumerate depth stream output modes. Reason: %s", xnGetStatusString (status));
00841   }
00842   depth_lock.unlock ();
00843 
00844   for (unsigned modeIdx = 0; modeIdx < mode_count; ++modeIdx)
00845     available_depth_modes_.push_back (modes[modeIdx]);
00846   delete[] modes;
00847  */
00848 }
00849 
00850 bool OpenNIDevice::isImageModeSupported (const XnMapOutputMode& output_mode) const throw (OpenNIException)
00851 {
00852   for (vector<XnMapOutputMode>::const_iterator modeIt = available_image_modes_.begin (); modeIt != available_image_modes_.end (); ++modeIt)
00853   {
00854     if (modeIt->nFPS == output_mode.nFPS && modeIt->nXRes == output_mode.nXRes && modeIt->nYRes == output_mode.nYRes)
00855       return true;
00856   }
00857   return false;
00858 }
00859 
00860 bool OpenNIDevice::isDepthModeSupported (const XnMapOutputMode& output_mode) const throw (OpenNIException)
00861 {
00862   for (vector<XnMapOutputMode>::const_iterator modeIt = available_depth_modes_.begin (); modeIt != available_depth_modes_.end (); ++modeIt)
00863   {
00864     if (modeIt->nFPS == output_mode.nFPS && modeIt->nXRes == output_mode.nXRes && modeIt->nYRes == output_mode.nYRes)
00865       return true;
00866   }
00867   return false;
00868 }
00869 
00870 const XnMapOutputMode&  OpenNIDevice::getDefaultImageMode () const throw ()
00871 {
00872   return available_image_modes_[0];
00873 }
00874 
00875 const XnMapOutputMode& OpenNIDevice::getDefaultDepthMode () const throw ()
00876 {
00877   return available_depth_modes_[0];
00878 }
00879 
00880 const XnMapOutputMode&  OpenNIDevice::getDefaultIRMode () const throw ()
00881 {
00883   return available_depth_modes_[0];
00884 }
00885 
00886 void OpenNIDevice::setImageOutputMode (const XnMapOutputMode& output_mode) throw (OpenNIException)
00887 {
00888   if (hasImageStream ())
00889   {
00890     lock_guard<mutex> image_lock (image_mutex_);
00891     XnStatus status = image_generator_.SetMapOutputMode (output_mode);
00892     if (status != XN_STATUS_OK)
00893       THROW_OPENNI_EXCEPTION ("Could not set image stream output mode to %dx%d@%d. Reason: %s", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS, xnGetStatusString (status));
00894   }
00895   else
00896     THROW_OPENNI_EXCEPTION ("Device does not provide an image stream");
00897 }
00898 
00899 void OpenNIDevice::setDepthOutputMode (const XnMapOutputMode& output_mode) throw (OpenNIException)
00900 {
00901   if (hasDepthStream ())
00902   {
00903     lock_guard<mutex> depth_lock (depth_mutex_);
00904     XnStatus status = depth_generator_.SetMapOutputMode (output_mode);
00905     if (status != XN_STATUS_OK)
00906       THROW_OPENNI_EXCEPTION ("Could not set depth stream output mode to %dx%d@%d. Reason: %s", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS, xnGetStatusString (status));
00907   }
00908   else
00909     THROW_OPENNI_EXCEPTION ("Device does not provide a depth stream");
00910 }
00911 
00912 void OpenNIDevice::setIROutputMode (const XnMapOutputMode& output_mode) throw (OpenNIException)
00913 {
00914   if (hasIRStream ())
00915   {
00916     lock_guard<mutex> ir_lock (ir_mutex_);
00917     XnStatus status = ir_generator_.SetMapOutputMode (output_mode);
00918     if (status != XN_STATUS_OK)
00919       THROW_OPENNI_EXCEPTION ("Could not set IR stream output mode to %dx%d@%d. Reason: %s", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS, xnGetStatusString (status));
00920   }
00921   else
00922     THROW_OPENNI_EXCEPTION ("Device does not provide an IR stream");
00923 }
00924 
00925 XnMapOutputMode OpenNIDevice::getImageOutputMode () const throw (OpenNIException)
00926 {
00927   if (!hasImageStream ())
00928     THROW_OPENNI_EXCEPTION ("Device does not provide an image stream");
00929 
00930   XnMapOutputMode output_mode;
00931   lock_guard<mutex> image_lock (image_mutex_);
00932   XnStatus status = image_generator_.GetMapOutputMode (output_mode);
00933   if (status != XN_STATUS_OK)
00934     THROW_OPENNI_EXCEPTION ("Could not get image stream output mode. Reason: %s", xnGetStatusString (status));
00935   return output_mode;
00936 }
00937 
00938 XnMapOutputMode OpenNIDevice::getDepthOutputMode () const throw (OpenNIException)
00939 {
00940   if (!hasDepthStream () )
00941     THROW_OPENNI_EXCEPTION ("Device does not provide a depth stream");
00942 
00943   XnMapOutputMode output_mode;
00944   lock_guard<mutex> depth_lock (depth_mutex_);
00945   XnStatus status = depth_generator_.GetMapOutputMode (output_mode);
00946   if (status != XN_STATUS_OK)
00947     THROW_OPENNI_EXCEPTION ("Could not get depth stream output mode. Reason: %s", xnGetStatusString (status));
00948   return output_mode;
00949 }
00950 
00951 XnMapOutputMode OpenNIDevice::getIROutputMode () const throw (OpenNIException)
00952 {
00953   if (!hasIRStream ())
00954     THROW_OPENNI_EXCEPTION ("Device does not provide an IR stream");
00955 
00956   XnMapOutputMode output_mode;
00957   lock_guard<mutex> ir_lock (ir_mutex_);
00958   XnStatus status = ir_generator_.GetMapOutputMode (output_mode);
00959   if (status != XN_STATUS_OK)
00960     THROW_OPENNI_EXCEPTION ("Could not get IR stream output mode. Reason: %s", xnGetStatusString (status));
00961   return output_mode;
00962 }
00963 
00964 // This magic value is taken from a calibration routine, unless calibrated params are not supported we rely on thsi value!
00965 const float OpenNIDevice::rgb_focal_length_SXGA_ = 1050;
00966 } // namespace


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