Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * Author: Julius Kammerl (julius@kammerl.de) 00035 */ 00036 00037 #ifndef POINT_COMPRESSION_H 00038 #define POINT_COMPRESSION_H 00039 00040 #include <iterator> 00041 #include <iostream> 00042 #include <vector> 00043 #include <string.h> 00044 #include <iostream> 00045 #include <stdio.h> 00046 #include <string.h> 00047 00048 namespace pcl 00049 { 00050 namespace octree 00051 { 00052 using namespace std; 00053 00055 00061 00062 template<typename PointT> 00063 class PointCoding 00064 { 00065 00066 // public typedefs 00067 typedef pcl::PointCloud<PointT> PointCloud; 00068 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00069 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00070 00071 public: 00072 00076 PointCoding () 00077 { 00078 pointCompressionResolution_ = 0.001; // 1mm 00079 } 00080 00082 virtual 00083 ~PointCoding () 00084 { 00085 } 00086 00090 inline 00091 void 00092 setPrecision (float precision_arg) 00093 { 00094 pointCompressionResolution_ = precision_arg; 00095 } 00096 00100 inline 00101 float 00102 getPrecision () 00103 { 00104 return pointCompressionResolution_; 00105 } 00106 00110 inline 00111 void 00112 setPointCount (unsigned int pointCount_arg) 00113 { 00114 pointDiffDataVector_.reserve (pointCount_arg * 3); 00115 } 00116 00119 void 00120 initializeEncoding () 00121 { 00122 pointDiffDataVector_.clear (); 00123 } 00124 00127 void 00128 initializeDecoding () 00129 { 00130 pointDiffDataVectorIterator_ = pointDiffDataVector_.begin (); 00131 } 00132 00135 std::vector<char>& 00136 getDifferentialDataVector () 00137 { 00138 return pointDiffDataVector_; 00139 } 00140 00146 void 00147 encodePoints (const typename std::vector<int>& indexVector_arg, const double* referencePoint_arg, 00148 PointCloudConstPtr inputCloud_arg) 00149 { 00150 std::size_t i, len; 00151 00152 len = indexVector_arg.size (); 00153 00154 // iterate over points within current voxel 00155 for (i = 0; i < len; i++) 00156 { 00157 unsigned char diffX, diffY, diffZ; 00158 00159 // retrieve point from cloud 00160 const int& idx = indexVector_arg[i]; 00161 const PointT& idxPoint = inputCloud_arg->points[idx]; 00162 00163 // differentially encode point coordinates and truncate overflow 00164 diffX = (unsigned char) max(-127, min<int>(127, (int)((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_))); 00165 diffY = (unsigned char) max(-127, min<int>(127, (int)((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_))); 00166 diffZ = (unsigned char) max(-127, min<int>(127, (int)((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_))); 00167 00168 // store information in differential point vector 00169 pointDiffDataVector_.push_back ( diffX ); 00170 pointDiffDataVector_.push_back ( diffY ); 00171 pointDiffDataVector_.push_back ( diffZ ); 00172 00173 } 00174 00175 } 00176 00183 void 00184 decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg, 00185 std::size_t endIdx_arg) 00186 { 00187 std::size_t i; 00188 unsigned int pointCount; 00189 00190 assert (beginIdx_arg <= endIdx_arg); 00191 00192 pointCount = endIdx_arg - beginIdx_arg; 00193 00194 // iterate over points within current voxel 00195 for (i = 0; i < pointCount; i++) 00196 { 00197 // retrieve differential point information 00198 const unsigned char& diffX = (unsigned char)*(pointDiffDataVectorIterator_++); 00199 const unsigned char& diffY = (unsigned char)*(pointDiffDataVectorIterator_++); 00200 const unsigned char& diffZ = (unsigned char)*(pointDiffDataVectorIterator_++); 00201 00202 // retrieve point from point cloud 00203 PointT& point = outputCloud_arg->points[beginIdx_arg + i]; 00204 00205 // decode point position 00206 point.x = referencePoint_arg[0] + (((float)diffX) * pointCompressionResolution_); 00207 point.y = referencePoint_arg[1] + (((float)diffY) * pointCompressionResolution_); 00208 point.z = referencePoint_arg[2] + (((float)diffZ) * pointCompressionResolution_); 00209 } 00210 00211 } 00212 00213 protected: 00214 00216 PointCloudPtr output_; 00217 00219 std::vector<char> pointDiffDataVector_; 00220 00222 std::vector<char>::const_iterator pointDiffDataVectorIterator_; 00223 00225 float pointCompressionResolution_; 00226 00227 00228 00229 }; 00230 00231 } 00232 00233 } 00234 00235 #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>; 00236 00237 #endif