Point Cloud Library (PCL)  1.3.1
color_coding.h
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines