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) 2010-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 COLOR_COMPRESSION_H 00039 #define COLOR_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 using namespace std; 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00056 /** \brief @b ColorCoding class 00057 * \note This class encodes 8-bit color information for octree-based point cloud compression. 00058 * \note 00059 * \note typename: PointT: type of point used in pointcloud 00060 * \author Julius Kammerl (julius@kammerl.de) 00061 */ 00062 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00063 template<typename PointT> 00064 class ColorCoding 00065 { 00066 00067 // public typedefs 00068 typedef pcl::PointCloud<PointT> PointCloud; 00069 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00070 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00071 00072 public: 00073 00074 /** \brief Constructor. 00075 * 00076 * */ 00077 ColorCoding () : 00078 output_ (), pointAvgColorDataVector_ (), pointAvgColorDataVector_Iterator_ (), 00079 pointDiffColorDataVector_ (), pointDiffColorDataVector_Iterator_ (), colorBitReduction_ (0) 00080 { 00081 } 00082 00083 /** \brief Empty class constructor. */ 00084 virtual 00085 ~ColorCoding () 00086 { 00087 } 00088 00089 /** \brief Define color bit depth of encoded color information. 00090 * \param bitDepth_arg: amounts of bits for representing one color component 00091 */ 00092 inline 00093 void 00094 setBitDepth (unsigned char bitDepth_arg) 00095 { 00096 colorBitReduction_ = static_cast<unsigned char> (8 - bitDepth_arg); 00097 } 00098 00099 /** \brief Retrieve color bit depth of encoded color information. 00100 * \return amounts of bits for representing one color component 00101 */ 00102 inline unsigned char 00103 getBitDepth () 00104 { 00105 return (static_cast<unsigned char> (8 - colorBitReduction_)); 00106 } 00107 00108 /** \brief Set amount of voxels containing point color information and reserve memory 00109 * \param voxelCount_arg: amounts of voxels 00110 */ 00111 inline void 00112 setVoxelCount (unsigned int voxelCount_arg) 00113 { 00114 pointAvgColorDataVector_.reserve (voxelCount_arg * 3); 00115 } 00116 00117 /** \brief Set amount of points within point cloud to be encoded and reserve memory 00118 * \param pointCount_arg: amounts of points within point cloud 00119 * */ 00120 inline 00121 void 00122 setPointCount (unsigned int pointCount_arg) 00123 { 00124 pointDiffColorDataVector_.reserve (pointCount_arg * 3); 00125 } 00126 00127 /** \brief Initialize encoding of color information 00128 * */ 00129 void 00130 initializeEncoding () 00131 { 00132 pointAvgColorDataVector_.clear (); 00133 00134 pointDiffColorDataVector_.clear (); 00135 } 00136 00137 /** \brief Initialize decoding of color information 00138 * */ 00139 void 00140 initializeDecoding () 00141 { 00142 pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin (); 00143 00144 pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin (); 00145 } 00146 00147 /** \brief Get reference to vector containing averaged color data 00148 * */ 00149 std::vector<char>& 00150 getAverageDataVector () 00151 { 00152 return pointAvgColorDataVector_; 00153 } 00154 00155 /** \brief Get reference to vector containing differential color data 00156 * */ 00157 std::vector<char>& 00158 getDifferentialDataVector () 00159 { 00160 return pointDiffColorDataVector_; 00161 } 00162 00163 /** \brief Encode averaged color information for a subset of points from point cloud 00164 * \param indexVector_arg indices defining a subset of points from points cloud 00165 * \param rgba_offset_arg offset to color information 00166 * \param inputCloud_arg input point cloud 00167 * */ 00168 void 00169 encodeAverageOfPoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) 00170 { 00171 std::size_t i, len; 00172 00173 unsigned int avgRed; 00174 unsigned int avgGreen; 00175 unsigned int avgBlue; 00176 00177 // initialize 00178 avgRed = avgGreen = avgBlue = 0; 00179 00180 // iterate over points 00181 len = indexVector_arg.size (); 00182 for (i = 0; i < len; i++) 00183 { 00184 // get color information from points 00185 const int& idx = indexVector_arg[i]; 00186 const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]); 00187 const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg); 00188 00189 // add color information 00190 avgRed += (colorInt >> 0) & 0xFF; 00191 avgGreen += (colorInt >> 8) & 0xFF; 00192 avgBlue += (colorInt >> 16) & 0xFF; 00193 00194 } 00195 00196 // calculated average color information 00197 if (len > 1) 00198 { 00199 avgRed /= static_cast<unsigned int> (len); 00200 avgGreen /= static_cast<unsigned int> (len); 00201 avgBlue /= static_cast<unsigned int> (len); 00202 } 00203 00204 // remove least significant bits 00205 avgRed >>= colorBitReduction_; 00206 avgGreen >>= colorBitReduction_; 00207 avgBlue >>= colorBitReduction_; 00208 00209 // add to average color vector 00210 pointAvgColorDataVector_.push_back (static_cast<char> (avgRed)); 00211 pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen)); 00212 pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue)); 00213 } 00214 00215 /** \brief Encode color information of a subset of points from point cloud 00216 * \param indexVector_arg indices defining a subset of points from points cloud 00217 * \param rgba_offset_arg offset to color information 00218 * \param inputCloud_arg input point cloud 00219 * */ 00220 void 00221 encodePoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) 00222 { 00223 std::size_t i, len; 00224 00225 unsigned int avgRed; 00226 unsigned int avgGreen; 00227 unsigned int avgBlue; 00228 00229 // initialize 00230 avgRed = avgGreen = avgBlue = 0; 00231 00232 // iterate over points 00233 len = indexVector_arg.size (); 00234 for (i = 0; i < len; i++) 00235 { 00236 // get color information from point 00237 const int& idx = indexVector_arg[i]; 00238 const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]); 00239 const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg); 00240 00241 // add color information 00242 avgRed += (colorInt >> 0) & 0xFF; 00243 avgGreen += (colorInt >> 8) & 0xFF; 00244 avgBlue += (colorInt >> 16) & 0xFF; 00245 00246 } 00247 00248 if (len > 1) 00249 { 00250 unsigned char diffRed; 00251 unsigned char diffGreen; 00252 unsigned char diffBlue; 00253 00254 // calculated average color information 00255 avgRed /= static_cast<unsigned int> (len); 00256 avgGreen /= static_cast<unsigned int> (len); 00257 avgBlue /= static_cast<unsigned int> (len); 00258 00259 // iterate over points for differential encoding 00260 for (i = 0; i < len; i++) 00261 { 00262 const int& idx = indexVector_arg[i]; 00263 const char* idxPointPtr = reinterpret_cast<const char*> (&inputCloud_arg->points[idx]); 00264 const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg); 00265 00266 // extract color components and do XOR encoding with predicted average color 00267 diffRed = (static_cast<unsigned char> (avgRed)) ^ static_cast<unsigned char> (((colorInt >> 0) & 0xFF)); 00268 diffGreen = (static_cast<unsigned char> (avgGreen)) ^ static_cast<unsigned char> (((colorInt >> 8) & 0xFF)); 00269 diffBlue = (static_cast<unsigned char> (avgBlue)) ^ static_cast<unsigned char> (((colorInt >> 16) & 0xFF)); 00270 00271 // remove least significant bits 00272 diffRed = static_cast<unsigned char> (diffRed >> colorBitReduction_); 00273 diffGreen = static_cast<unsigned char> (diffGreen >> colorBitReduction_); 00274 diffBlue = static_cast<unsigned char> (diffBlue >> colorBitReduction_); 00275 00276 // add to differential color vector 00277 pointDiffColorDataVector_.push_back (static_cast<char> (diffRed)); 00278 pointDiffColorDataVector_.push_back (static_cast<char> (diffGreen)); 00279 pointDiffColorDataVector_.push_back (static_cast<char> (diffBlue)); 00280 } 00281 } 00282 00283 // remove least significant bits from average color information 00284 avgRed >>= colorBitReduction_; 00285 avgGreen >>= colorBitReduction_; 00286 avgBlue >>= colorBitReduction_; 00287 00288 // add to differential color vector 00289 pointAvgColorDataVector_.push_back (static_cast<char> (avgRed)); 00290 pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen)); 00291 pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue)); 00292 00293 } 00294 00295 /** \brief Decode color information 00296 * \param outputCloud_arg output point cloud 00297 * \param beginIdx_arg index indicating first point to be assiged with color information 00298 * \param endIdx_arg index indicating last point to be assiged with color information 00299 * \param rgba_offset_arg offset to color information 00300 */ 00301 void 00302 decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) 00303 { 00304 std::size_t i; 00305 unsigned int pointCount; 00306 unsigned int colorInt; 00307 00308 assert (beginIdx_arg <= endIdx_arg); 00309 00310 // amount of points to be decoded 00311 pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg); 00312 00313 // get averaged color information for current voxel 00314 unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++); 00315 unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++); 00316 unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++); 00317 00318 // invert bit shifts during encoding 00319 avgRed = static_cast<unsigned char> (avgRed << colorBitReduction_); 00320 avgGreen = static_cast<unsigned char> (avgGreen << colorBitReduction_); 00321 avgBlue = static_cast<unsigned char> (avgBlue << colorBitReduction_); 00322 00323 // iterate over points 00324 for (i = 0; i < pointCount; i++) 00325 { 00326 if (pointCount > 1) 00327 { 00328 // get differential color information from input vector 00329 unsigned char diffRed = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++)); 00330 unsigned char diffGreen = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++)); 00331 unsigned char diffBlue = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++)); 00332 00333 // invert bit shifts during encoding 00334 diffRed = static_cast<unsigned char> (diffRed << colorBitReduction_); 00335 diffGreen = static_cast<unsigned char> (diffGreen << colorBitReduction_); 00336 diffBlue = static_cast<unsigned char> (diffBlue << colorBitReduction_); 00337 00338 // decode color information 00339 colorInt = ((avgRed ^ diffRed) << 0) | 00340 ((avgGreen ^ diffGreen) << 8) | 00341 ((avgBlue ^ diffBlue) << 16); 00342 } 00343 else 00344 { 00345 // decode color information 00346 colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16); 00347 } 00348 00349 char* idxPointPtr = reinterpret_cast<char*> (&outputCloud_arg->points[beginIdx_arg + i]); 00350 int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg); 00351 // assign color to point from point cloud 00352 pointColor=colorInt; 00353 } 00354 } 00355 00356 /** \brief Set default color to points 00357 * \param outputCloud_arg output point cloud 00358 * \param beginIdx_arg index indicating first point to be assiged with color information 00359 * \param endIdx_arg index indicating last point to be assiged with color information 00360 * \param rgba_offset_arg offset to color information 00361 * */ 00362 void 00363 setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) 00364 { 00365 std::size_t i; 00366 unsigned int pointCount; 00367 00368 assert (beginIdx_arg <= endIdx_arg); 00369 00370 // amount of points to be decoded 00371 pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg); 00372 00373 // iterate over points 00374 for (i = 0; i < pointCount; i++) 00375 { 00376 char* idxPointPtr = reinterpret_cast<char*> (&outputCloud_arg->points[beginIdx_arg + i]); 00377 int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg); 00378 // assign color to point from point cloud 00379 pointColor = defaultColor_; 00380 } 00381 } 00382 00383 00384 protected: 00385 00386 /** \brief Pointer to output point cloud dataset. */ 00387 PointCloudPtr output_; 00388 00389 /** \brief Vector for storing average color information */ 00390 std::vector<char> pointAvgColorDataVector_; 00391 00392 /** \brief Iterator on average color information vector */ 00393 std::vector<char>::const_iterator pointAvgColorDataVector_Iterator_; 00394 00395 /** \brief Vector for storing differential color information */ 00396 std::vector<char> pointDiffColorDataVector_; 00397 00398 /** \brief Iterator on differential color information vector */ 00399 std::vector<char>::const_iterator pointDiffColorDataVector_Iterator_; 00400 00401 /** \brief Amount of bits to be removed from color components before encoding */ 00402 unsigned char colorBitReduction_; 00403 00404 // frame header identifier 00405 static const int defaultColor_; 00406 00407 }; 00408 00409 // define default color 00410 template<typename PointT> 00411 const int ColorCoding<PointT>::defaultColor_ = ((255) << 0) | 00412 ((255) << 8) | 00413 ((255) << 16); 00414 00415 } 00416 } 00417 00418 #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>; 00419 00420 #endif