Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2011 Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 #include <pcl/pcl_config.h> 00039 #ifdef HAVE_OPENNI 00040 00041 #ifndef __OPENNI_DEPTH_IMAGE__ 00042 #define __OPENNI_DEPTH_IMAGE__ 00043 00044 #include "openni.h" 00045 00046 //#include <pcl/pcl_macros.h> // <-- because current header is included in NVCC-compiled code and contains <Eigen/Core>. Consider <pcl/pcl_exports.h> 00047 #include <pcl/pcl_exports.h> 00048 #include "openni_exception.h" 00049 #include <pcl/io/boost.h> 00050 00051 namespace openni_wrapper 00052 { 00053 /** \brief This class provides methods to fill a depth or disparity image. 00054 * \author Suat Gedikli 00055 */ 00056 class PCL_EXPORTS DepthImage 00057 { 00058 public: 00059 typedef boost::shared_ptr<DepthImage> Ptr; 00060 typedef boost::shared_ptr<const DepthImage> ConstPtr; 00061 00062 /** \brief Constructor 00063 * \param[in] depth_meta_data the actual data from the OpenNI library 00064 * \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for 00065 * Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design. 00066 * \param[in] focal_length focal length of the "stereo" frame. 00067 * \param[in] shadow_value defines which values in the depth data are indicating shadow (resulting from the parallax between projector and IR camera) 00068 * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . 00069 * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. 00070 */ 00071 inline DepthImage (boost::shared_ptr<xn::DepthMetaData> depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) throw (); 00072 00073 /** \brief Destructor. Never throws an exception. */ 00074 inline virtual ~DepthImage () throw (); 00075 00076 /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method 00077 * \return the actual depth data of type xn::DepthMetaData. 00078 */ 00079 inline const xn::DepthMetaData& 00080 getDepthMetaData () const throw (); 00081 00082 /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. 00083 * \param[in] width the width of the desired disparity image. 00084 * \param[in] height the height of the desired disparity image. 00085 * \param[in,out] disparity_buffer the float pointer to the actual memory buffer to be filled with the disparity values. 00086 * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the 00087 * width in bytes (not floats) of the original width of the depth buffer. 00088 */ 00089 void 00090 fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step = 0) const; 00091 00092 /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. 00093 * \param[in] width width the width of the desired depth image. 00094 * \param[in] height height the height of the desired depth image. 00095 * \param[in,out] depth_buffer the float pointer to the actual memory buffer to be filled with the depth values. 00096 * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the 00097 * width in bytes (not floats) of the original width of the depth buffer. 00098 */ 00099 void 00100 fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step = 0) const; 00101 00102 /** \brief fills a user given block of memory with the raw values with additional nearest-neighbor down-scaling. 00103 * \param[in] width width the width of the desired raw image. 00104 * \param[in] height height the height of the desired raw image. 00105 * \param[in,out] depth_buffer the unsigned short pointer to the actual memory buffer to be filled with the raw values. 00106 * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the 00107 * width in bytes (not floats) of the original width of the depth buffer. 00108 */ 00109 void 00110 fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step = 0) const; 00111 00112 /** \brief method to access the baseline of the "stereo" frame that was used to retrieve the depth image. 00113 * \return baseline in meters 00114 */ 00115 inline float 00116 getBaseline () const throw (); 00117 00118 /** \brief method to access the focal length of the "stereo" frame that was used to retrieve the depth image. 00119 * \return focal length in pixels 00120 */ 00121 inline float 00122 getFocalLength () const throw (); 00123 00124 /** \brief method to access the shadow value, that indicates pixels lying in shadow in the depth image. 00125 * \return shadow value 00126 */ 00127 inline XnUInt64 00128 getShadowValue () const throw (); 00129 00130 /** \brief method to access the no-sample value, that indicates pixels where no disparity could be determined for the depth image. 00131 * \return no-sample value 00132 */ 00133 inline XnUInt64 00134 getNoSampleValue () const throw (); 00135 00136 /** \return the width of the depth image */ 00137 inline unsigned 00138 getWidth () const throw (); 00139 00140 /** \return the height of the depth image */ 00141 inline unsigned 00142 getHeight () const throw (); 00143 00144 /** \return an ascending id for the depth frame 00145 * \attention not necessarily synchronized with other streams 00146 */ 00147 inline unsigned 00148 getFrameID () const throw (); 00149 00150 /** \return a ascending timestamp for the depth frame 00151 * \attention its not the system time, thus can not be used directly to synchronize different sensors. 00152 * But definitely synchronized with other streams 00153 */ 00154 inline unsigned long 00155 getTimeStamp () const throw (); 00156 00157 protected: 00158 boost::shared_ptr<xn::DepthMetaData> depth_md_; 00159 float baseline_; 00160 float focal_length_; 00161 XnUInt64 shadow_value_; 00162 XnUInt64 no_sample_value_; 00163 } ; 00164 00165 DepthImage::DepthImage (boost::shared_ptr<xn::DepthMetaData> depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) throw () 00166 : depth_md_ (depth_meta_data) 00167 , baseline_ (baseline) 00168 , focal_length_ (focal_length) 00169 , shadow_value_ (shadow_value) 00170 , no_sample_value_ (no_sample_value) { } 00171 00172 DepthImage::~DepthImage () throw () { } 00173 00174 const xn::DepthMetaData& 00175 DepthImage::getDepthMetaData () const throw () 00176 { 00177 return *depth_md_; 00178 } 00179 00180 float 00181 DepthImage::getBaseline () const throw () 00182 { 00183 return baseline_; 00184 } 00185 00186 float 00187 DepthImage::getFocalLength () const throw () 00188 { 00189 return focal_length_; 00190 } 00191 00192 XnUInt64 00193 DepthImage::getShadowValue () const throw () 00194 { 00195 return shadow_value_; 00196 } 00197 00198 XnUInt64 00199 DepthImage::getNoSampleValue () const throw () 00200 { 00201 return no_sample_value_; 00202 } 00203 00204 unsigned 00205 DepthImage::getWidth () const throw () 00206 { 00207 return depth_md_->XRes (); 00208 } 00209 00210 unsigned 00211 DepthImage::getHeight () const throw () 00212 { 00213 return depth_md_->YRes (); 00214 } 00215 00216 unsigned 00217 DepthImage::getFrameID () const throw () 00218 { 00219 return depth_md_->FrameID (); 00220 } 00221 00222 unsigned long 00223 DepthImage::getTimeStamp () const throw () 00224 { 00225 return static_cast<unsigned long> (depth_md_->Timestamp ()); 00226 } 00227 } // namespace 00228 #endif 00229 #endif //__OPENNI_DEPTH_IMAGE