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_SHIFT_TO_DEPTH_CONVERSION 00042 #define __OPENNI_SHIFT_TO_DEPTH_CONVERSION 00043 00044 #include <vector> 00045 #include <limits> 00046 00047 namespace openni_wrapper 00048 { 00049 /** \brief This class provides conversion of the openni 11-bit shift data to depth; 00050 */ 00051 class PCL_EXPORTS ShiftToDepthConverter 00052 { 00053 public: 00054 /** \brief Constructor. */ 00055 ShiftToDepthConverter () : init_(false) {} 00056 00057 /** \brief Destructor. */ 00058 virtual ~ShiftToDepthConverter () {}; 00059 00060 /** \brief This method generates a look-up table to convert openni shift values to depth 00061 */ 00062 void 00063 generateLookupTable () 00064 { 00065 // lookup of 11 bit shift values 00066 const std::size_t table_size = 1<<10; 00067 00068 lookupTable_.clear(); 00069 lookupTable_.resize(table_size); 00070 00071 // constants taken from openni driver 00072 static const int16_t nConstShift = 800; 00073 static const double nParamCoeff = 4.000000; 00074 static const double dPlanePixelSize = 0.104200; 00075 static const double nShiftScale = 10.000000; 00076 static const double dPlaneDsr = 120.000000; 00077 static const double dPlaneDcl = 7.500000; 00078 00079 std::size_t i; 00080 double dFixedRefX; 00081 double dMetric; 00082 00083 for (i=0; i<table_size; ++i) 00084 { 00085 // shift to depth calculation from opnni 00086 dFixedRefX = (static_cast<double>(i - nConstShift) / nParamCoeff)-0.375; 00087 dMetric = dFixedRefX * dPlanePixelSize; 00088 lookupTable_[i] = static_cast<float>((nShiftScale * ((dMetric * dPlaneDsr / (dPlaneDcl - dMetric)) + dPlaneDsr) ) / 1000.0f); 00089 } 00090 00091 init_ = true; 00092 } 00093 00094 /** \brief Generate a look-up table for converting openni shift values to depth 00095 */ 00096 inline float 00097 shiftToDepth (uint16_t shift_val) 00098 { 00099 assert (init_); 00100 00101 static const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00102 00103 float ret = bad_point; 00104 00105 // lookup depth value in shift lookup table 00106 if (shift_val<lookupTable_.size()) 00107 ret = lookupTable_[shift_val]; 00108 00109 return ret; 00110 } 00111 00112 inline bool isInitialized() const 00113 { 00114 return init_; 00115 } 00116 00117 protected: 00118 std::vector<float> lookupTable_; 00119 bool init_; 00120 } ; 00121 } 00122 00123 #endif 00124 #endif //__OPENNI_SHIFT_TO_DEPTH_CONVERSION