Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <openni_camera/openni_device_kinect.h>
00038 #include <openni_camera/openni_image_bayer_grbg.h>
00039 #include <iostream>
00040 #include <sstream>
00041 #include <boost/thread/mutex.hpp>
00042
00043 using namespace boost;
00044
00045 namespace openni_wrapper
00046 {
00047
00048 DeviceKinect::DeviceKinect (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)
00049 : OpenNIDevice (context, device_node, image_node, depth_node, ir_node)
00050 , debayering_method_ (ImageBayerGRBG::EdgeAwareWeighted)
00051 {
00052
00053 enumAvailableModes ();
00054 setDepthOutputMode (getDefaultDepthMode ());
00055 setImageOutputMode (getDefaultImageMode ());
00056 setIROutputMode (getDefaultIRMode ());
00057
00058
00059 XnStatus status;
00060
00061 unique_lock<mutex> image_lock(image_mutex_);
00062
00063 status = image_generator_.SetIntProperty ("InputFormat", 6);
00064 if (status != XN_STATUS_OK)
00065 THROW_OPENNI_EXCEPTION ("Error setting the image input format to Uncompressed 8-bit BAYER. Reason: %s", xnGetStatusString (status));
00066
00067
00068 status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_GRAYSCALE_8_BIT);
00069 if (status != XN_STATUS_OK)
00070 THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to 8bit-grayscale. Reason: %s", xnGetStatusString (status));
00071 image_lock.unlock ();
00072
00073 lock_guard<mutex> depth_lock(depth_mutex_);
00074
00075 status = depth_generator_.SetIntProperty ("RegistrationType", 2);
00076 if (status != XN_STATUS_OK)
00077 THROW_OPENNI_EXCEPTION ("Error setting the registration type. Reason: %s", xnGetStatusString (status));
00078 }
00079
00080 DeviceKinect::~DeviceKinect () throw ()
00081 {
00082 depth_mutex_.lock ();
00083 depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_);
00084 depth_mutex_.unlock ();
00085
00086 image_mutex_.lock ();
00087 image_generator_.UnregisterFromNewDataAvailable (image_callback_handle_);
00088 image_mutex_.unlock ();
00089 }
00090
00091 bool DeviceKinect::isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw ()
00092 {
00093 return ImageBayerGRBG::resizingSupported (input_width, input_height, output_width, output_height);
00094 }
00095
00096 void DeviceKinect::enumAvailableModes () throw (OpenNIException)
00097 {
00098 XnMapOutputMode output_mode;
00099 available_image_modes_.clear();
00100 available_depth_modes_.clear();
00101
00102 output_mode.nFPS = 30;
00103 output_mode.nXRes = XN_VGA_X_RES;
00104 output_mode.nYRes = XN_VGA_Y_RES;
00105 available_image_modes_.push_back (output_mode);
00106 available_depth_modes_.push_back (output_mode);
00107
00108 output_mode.nFPS = 15;
00109 output_mode.nXRes = XN_SXGA_X_RES;
00110 output_mode.nYRes = XN_SXGA_Y_RES;
00111 available_image_modes_.push_back (output_mode);
00112 }
00113
00114 boost::shared_ptr<Image> DeviceKinect::getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw ()
00115 {
00116 return boost::shared_ptr<Image> (new ImageBayerGRBG (image_data, debayering_method_));
00117 }
00118
00119 void DeviceKinect::setSynchronization (bool on_off) throw (OpenNIException)
00120 {
00121 if (on_off)
00122 THROW_OPENNI_EXCEPTION ("Microsoft Kinect does not support Hardware synchronization.");
00123 }
00124
00125 bool DeviceKinect::isSynchronized () const throw (OpenNIException)
00126 {
00127 return false;
00128 }
00129
00130 bool DeviceKinect::isSynchronizationSupported () const throw ()
00131 {
00132 return false;
00133 }
00134
00135 bool DeviceKinect::isDepthCropped () const throw (OpenNIException)
00136 {
00137 return false;
00138 }
00139
00140 void DeviceKinect::setDepthCropping (unsigned x, unsigned y, unsigned width, unsigned height) throw (OpenNIException)
00141 {
00142 if (width != 0 && height != 0)
00143 THROW_OPENNI_EXCEPTION ("Microsoft Kinect does not support cropping for the depth stream.");
00144 }
00145
00146 bool DeviceKinect::isDepthCroppingSupported () const throw ()
00147 {
00148 return false;
00149 }
00150
00151 }