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-2012, Willow Garage, Inc. 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 00038 #ifndef POINT_COMPRESSION_H 00039 #define POINT_COMPRESSION_H 00040 00041 #include <iterator> 00042 #include <iostream> 00043 #include <vector> 00044 #include <string.h> 00045 #include <iostream> 00046 #include <stdio.h> 00047 #include <string.h> 00048 00049 namespace pcl 00050 { 00051 namespace octree 00052 { 00053 /** \brief @b PointCoding class 00054 * \note This class encodes 8-bit differential point information for octree-based point cloud compression. 00055 * \note typename: PointT: type of point used in pointcloud 00056 * \author Julius Kammerl (julius@kammerl.de) 00057 */ 00058 template<typename PointT> 00059 class PointCoding 00060 { 00061 // public typedefs 00062 typedef pcl::PointCloud<PointT> PointCloud; 00063 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00064 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00065 00066 public: 00067 /** \brief Constructor. */ 00068 PointCoding () : 00069 output_ (), pointDiffDataVector_ (), pointDiffDataVectorIterator_ (), 00070 pointCompressionResolution_ (0.001f) // 1mm 00071 { 00072 } 00073 00074 /** \brief Empty class constructor. */ 00075 virtual 00076 ~PointCoding () 00077 { 00078 } 00079 00080 /** \brief Define precision of point information 00081 * \param precision_arg: precision 00082 */ 00083 inline void 00084 setPrecision (float precision_arg) 00085 { 00086 pointCompressionResolution_ = precision_arg; 00087 } 00088 00089 /** \brief Retrieve precision of point information 00090 * \return precision 00091 */ 00092 inline float 00093 getPrecision () 00094 { 00095 return (pointCompressionResolution_); 00096 } 00097 00098 /** \brief Set amount of points within point cloud to be encoded and reserve memory 00099 * \param pointCount_arg: amounts of points within point cloud 00100 */ 00101 inline void 00102 setPointCount (unsigned int pointCount_arg) 00103 { 00104 pointDiffDataVector_.reserve (pointCount_arg * 3); 00105 } 00106 00107 /** \brief Initialize encoding of differential point */ 00108 void 00109 initializeEncoding () 00110 { 00111 pointDiffDataVector_.clear (); 00112 } 00113 00114 /** \brief Initialize decoding of differential point */ 00115 void 00116 initializeDecoding () 00117 { 00118 pointDiffDataVectorIterator_ = pointDiffDataVector_.begin (); 00119 } 00120 00121 /** \brief Get reference to vector containing differential color data */ 00122 std::vector<char>& 00123 getDifferentialDataVector () 00124 { 00125 return (pointDiffDataVector_); 00126 } 00127 00128 /** \brief Encode differential point information for a subset of points from point cloud 00129 * \param indexVector_arg indices defining a subset of points from points cloud 00130 * \param referencePoint_arg coordinates of reference point 00131 * \param inputCloud_arg input point cloud 00132 */ 00133 void 00134 encodePoints (const typename std::vector<int>& indexVector_arg, const double* referencePoint_arg, 00135 PointCloudConstPtr inputCloud_arg) 00136 { 00137 std::size_t i, len; 00138 00139 len = indexVector_arg.size (); 00140 00141 // iterate over points within current voxel 00142 for (i = 0; i < len; i++) 00143 { 00144 unsigned char diffX, diffY, diffZ; 00145 00146 // retrieve point from cloud 00147 const int& idx = indexVector_arg[i]; 00148 const PointT& idxPoint = inputCloud_arg->points[idx]; 00149 00150 // differentially encode point coordinates and truncate overflow 00151 diffX = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_)))); 00152 diffY = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_)))); 00153 diffZ = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_)))); 00154 00155 // store information in differential point vector 00156 pointDiffDataVector_.push_back (diffX); 00157 pointDiffDataVector_.push_back (diffY); 00158 pointDiffDataVector_.push_back (diffZ); 00159 } 00160 } 00161 00162 /** \brief Decode differential point information 00163 * \param outputCloud_arg output point cloud 00164 * \param referencePoint_arg coordinates of reference point 00165 * \param beginIdx_arg index indicating first point to be assiged with color information 00166 * \param endIdx_arg index indicating last point to be assiged with color information 00167 */ 00168 void 00169 decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg, 00170 std::size_t endIdx_arg) 00171 { 00172 std::size_t i; 00173 unsigned int pointCount; 00174 00175 assert (beginIdx_arg <= endIdx_arg); 00176 00177 pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg); 00178 00179 // iterate over points within current voxel 00180 for (i = 0; i < pointCount; i++) 00181 { 00182 // retrieve differential point information 00183 const unsigned char& diffX = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++)); 00184 const unsigned char& diffY = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++)); 00185 const unsigned char& diffZ = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++)); 00186 00187 // retrieve point from point cloud 00188 PointT& point = outputCloud_arg->points[beginIdx_arg + i]; 00189 00190 // decode point position 00191 point.x = static_cast<float> (referencePoint_arg[0] + diffX * pointCompressionResolution_); 00192 point.y = static_cast<float> (referencePoint_arg[1] + diffY * pointCompressionResolution_); 00193 point.z = static_cast<float> (referencePoint_arg[2] + diffZ * pointCompressionResolution_); 00194 } 00195 } 00196 00197 protected: 00198 /** \brief Pointer to output point cloud dataset. */ 00199 PointCloudPtr output_; 00200 00201 /** \brief Vector for storing differential point information */ 00202 std::vector<char> pointDiffDataVector_; 00203 00204 /** \brief Iterator on differential point information vector */ 00205 std::vector<char>::const_iterator pointDiffDataVectorIterator_; 00206 00207 /** \brief Precision of point coding*/ 00208 float pointCompressionResolution_; 00209 }; 00210 } 00211 } 00212 00213 #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>; 00214 00215 #endif