Point Cloud Library (PCL)  1.3.1
point_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 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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines