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 COLOR_COMPRESSION_H 00038 #define COLOR_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 ColorCoding 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 ColorCoding () : 00077 colorBitReduction_ (0) 00078 { 00079 } 00080 00082 virtual 00083 ~ColorCoding () 00084 { 00085 } 00086 00090 inline 00091 void 00092 setBitDepth (unsigned char bitDepth_arg) 00093 { 00094 assert (bitDepth_arg <= bitDepth_arg); 00095 colorBitReduction_ = 8 - bitDepth_arg; 00096 } 00097 00101 inline 00102 unsigned char 00103 getBitDepth () 00104 { 00105 return 8 - colorBitReduction_; 00106 } 00107 00111 inline 00112 void 00113 setVoxelCount (unsigned int voxelCount_arg) 00114 { 00115 pointAvgColorDataVector_.reserve (voxelCount_arg * 3); 00116 } 00117 00121 inline 00122 void 00123 setPointCount (unsigned int pointCount_arg) 00124 { 00125 pointDiffColorDataVector_.reserve (pointCount_arg * 3); 00126 } 00127 00130 void 00131 initializeEncoding () 00132 { 00133 pointAvgColorDataVector_.clear (); 00134 00135 pointDiffColorDataVector_.clear (); 00136 } 00137 00140 void 00141 initializeDecoding () 00142 { 00143 pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin (); 00144 00145 pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin (); 00146 } 00147 00150 std::vector<char>& 00151 getAverageDataVector () 00152 { 00153 return pointAvgColorDataVector_; 00154 } 00155 00158 std::vector<char>& 00159 getDifferentialDataVector () 00160 { 00161 return pointDiffColorDataVector_; 00162 } 00163 00169 void 00170 encodeAverageOfPoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) 00171 { 00172 std::size_t i, len; 00173 00174 unsigned int avgRed; 00175 unsigned int avgGreen; 00176 unsigned int avgBlue; 00177 00178 // initialize 00179 avgRed = avgGreen = avgBlue = 0; 00180 00181 // iterate over points 00182 len = indexVector_arg.size (); 00183 for (i = 0; i < len; i++) 00184 { 00185 // get color information from points 00186 const int& idx = indexVector_arg[i]; 00187 const char* idxPointPtr = (const char*) &inputCloud_arg->points[idx]; 00188 const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg); 00189 00190 // add color information 00191 avgRed += (colorInt >> 0) & 0xFF; 00192 avgGreen += (colorInt >> 8) & 0xFF; 00193 avgBlue += (colorInt >> 16) & 0xFF; 00194 00195 } 00196 00197 // calculated average color information 00198 if (len > 1) 00199 { 00200 avgRed /= len; 00201 avgGreen /= len; 00202 avgBlue /= len; 00203 } 00204 00205 // remove least significant bits 00206 avgRed >>= colorBitReduction_; 00207 avgGreen >>= colorBitReduction_; 00208 avgBlue >>= colorBitReduction_; 00209 00210 // add to average color vector 00211 pointAvgColorDataVector_.push_back ((char)avgRed); 00212 pointAvgColorDataVector_.push_back ((char)avgGreen); 00213 pointAvgColorDataVector_.push_back ((char)avgBlue); 00214 } 00215 00221 void 00222 encodePoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) 00223 { 00224 std::size_t i, len; 00225 00226 unsigned int avgRed; 00227 unsigned int avgGreen; 00228 unsigned int avgBlue; 00229 00230 // initialize 00231 avgRed = avgGreen = avgBlue = 0; 00232 00233 // iterate over points 00234 len = indexVector_arg.size (); 00235 for (i = 0; i < len; i++) 00236 { 00237 // get color information from point 00238 const int& idx = indexVector_arg[i]; 00239 const char* idxPointPtr = (const char*) &inputCloud_arg->points[idx]; 00240 const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg); 00241 00242 // add color information 00243 avgRed += (colorInt >> 0) & 0xFF; 00244 avgGreen += (colorInt >> 8) & 0xFF; 00245 avgBlue += (colorInt >> 16) & 0xFF; 00246 00247 } 00248 00249 if (len > 1) 00250 { 00251 unsigned char diffRed; 00252 unsigned char diffGreen; 00253 unsigned char diffBlue; 00254 00255 // calculated average color information 00256 avgRed /= len; 00257 avgGreen /= len; 00258 avgBlue /= len; 00259 00260 // iterate over points for differential encoding 00261 for (i = 0; i < len; i++) 00262 { 00263 const int& idx = indexVector_arg[i]; 00264 const char* idxPointPtr = (const char*) &inputCloud_arg->points[idx]; 00265 const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg); 00266 00267 // extract color components and do XOR encoding with predicted average color 00268 diffRed = ((unsigned char)avgRed) ^ (unsigned char)((colorInt >> 0) & 0xFF); 00269 diffGreen = ((unsigned char)avgGreen) ^ (unsigned char)((colorInt >> 8) & 0xFF); 00270 diffBlue = ((unsigned char)avgBlue) ^ (unsigned char)((colorInt >> 16) & 0xFF); 00271 00272 // remove least significant bits 00273 diffRed >>= colorBitReduction_; 00274 diffGreen >>= colorBitReduction_; 00275 diffBlue >>= colorBitReduction_; 00276 00277 // add to differential color vector 00278 pointDiffColorDataVector_.push_back ((char)diffRed); 00279 pointDiffColorDataVector_.push_back ((char)diffGreen); 00280 pointDiffColorDataVector_.push_back ((char)diffBlue); 00281 } 00282 } 00283 00284 // remove least significant bits from average color information 00285 avgRed >>= colorBitReduction_; 00286 avgGreen >>= colorBitReduction_; 00287 avgBlue >>= colorBitReduction_; 00288 00289 // add to differential color vector 00290 pointAvgColorDataVector_.push_back ((char)avgRed); 00291 pointAvgColorDataVector_.push_back ((char)avgGreen); 00292 pointAvgColorDataVector_.push_back ((char)avgBlue); 00293 00294 } 00295 00302 void 00303 decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) 00304 { 00305 std::size_t i; 00306 unsigned int pointCount; 00307 unsigned int colorInt; 00308 00309 assert (beginIdx_arg <= endIdx_arg); 00310 00311 // amount of points to be decoded 00312 pointCount = endIdx_arg - beginIdx_arg; 00313 00314 // get averaged color information for current voxel 00315 unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++); 00316 unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++); 00317 unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++); 00318 00319 // invert bit shifts during encoding 00320 avgRed <<= colorBitReduction_; 00321 avgGreen <<= colorBitReduction_; 00322 avgBlue <<= colorBitReduction_; 00323 00324 // iterate over points 00325 for (i = 0; i < pointCount; i++) 00326 { 00327 if (pointCount > 1) 00328 { 00329 // get differential color information from input vector 00330 unsigned char diffRed = (unsigned char)*(pointDiffColorDataVector_Iterator_++); 00331 unsigned char diffGreen = (unsigned char)*(pointDiffColorDataVector_Iterator_++); 00332 unsigned char diffBlue = (unsigned char)*(pointDiffColorDataVector_Iterator_++); 00333 00334 // invert bit shifts during encoding 00335 diffRed <<= colorBitReduction_; 00336 diffGreen <<= colorBitReduction_; 00337 diffBlue <<= colorBitReduction_; 00338 00339 // decode color information 00340 colorInt = ((avgRed ^ diffRed) << 0) | 00341 ((avgGreen ^ diffGreen) << 8) | 00342 ((avgBlue ^ diffBlue) << 16); 00343 } 00344 else 00345 { 00346 // decode color information 00347 colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16); 00348 } 00349 00350 char* idxPointPtr = (char*) &outputCloud_arg->points[beginIdx_arg + i]; 00351 int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg); 00352 // assign color to point from point cloud 00353 pointColor=colorInt; 00354 } 00355 } 00356 00363 void 00364 setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) 00365 { 00366 std::size_t i; 00367 unsigned int pointCount; 00368 00369 assert (beginIdx_arg <= endIdx_arg); 00370 00371 // amount of points to be decoded 00372 pointCount = endIdx_arg - beginIdx_arg; 00373 00374 // iterate over points 00375 for (i = 0; i < pointCount; i++) 00376 { 00377 char* idxPointPtr = (char*) &outputCloud_arg->points[beginIdx_arg + i]; 00378 int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg); 00379 // assign color to point from point cloud 00380 pointColor=defaultColor_; 00381 00382 } 00383 } 00384 00385 00386 protected: 00387 00389 PointCloudPtr output_; 00390 00392 std::vector<char> pointAvgColorDataVector_; 00393 00395 std::vector<char>::const_iterator pointAvgColorDataVector_Iterator_; 00396 00398 std::vector<char> pointDiffColorDataVector_; 00399 00401 std::vector<char>::const_iterator pointDiffColorDataVector_Iterator_; 00402 00404 unsigned char colorBitReduction_; 00405 00406 // frame header identifier 00407 static const int defaultColor_; 00408 00409 }; 00410 00411 // define default color 00412 template<typename PointT> 00413 const int ColorCoding<PointT>::defaultColor_ = ((255) << 0) | 00414 ((255) << 8) | 00415 ((255) << 16); 00416 00417 } 00418 00419 } 00420 00421 #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>; 00422 00423 #endif