Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef POINT_COMPRESSION_H
00039 #define POINT_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 {
00058 template<typename PointT>
00059 class PointCoding
00060 {
00061
00062 typedef pcl::PointCloud<PointT> PointCloud;
00063 typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00064 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00065
00066 public:
00068 PointCoding () :
00069 output_ (), pointDiffDataVector_ (), pointDiffDataVectorIterator_ (),
00070 pointCompressionResolution_ (0.001f)
00071 {
00072 }
00073
00075 virtual
00076 ~PointCoding ()
00077 {
00078 }
00079
00083 inline void
00084 setPrecision (float precision_arg)
00085 {
00086 pointCompressionResolution_ = precision_arg;
00087 }
00088
00092 inline float
00093 getPrecision ()
00094 {
00095 return (pointCompressionResolution_);
00096 }
00097
00101 inline void
00102 setPointCount (unsigned int pointCount_arg)
00103 {
00104 pointDiffDataVector_.reserve (pointCount_arg * 3);
00105 }
00106
00108 void
00109 initializeEncoding ()
00110 {
00111 pointDiffDataVector_.clear ();
00112 }
00113
00115 void
00116 initializeDecoding ()
00117 {
00118 pointDiffDataVectorIterator_ = pointDiffDataVector_.begin ();
00119 }
00120
00122 std::vector<char>&
00123 getDifferentialDataVector ()
00124 {
00125 return (pointDiffDataVector_);
00126 }
00127
00133 void
00134 encodePoints (const typename std::vector<int>& indexVector_arg, const double* referencePoint_arg,
00135 PointCloudConstPtr inputCloud_arg)
00136 {
00137 std::size_t i, len;
00138
00139 len = indexVector_arg.size ();
00140
00141
00142 for (i = 0; i < len; i++)
00143 {
00144 unsigned char diffX, diffY, diffZ;
00145
00146
00147 const int& idx = indexVector_arg[i];
00148 const PointT& idxPoint = inputCloud_arg->points[idx];
00149
00150
00151 diffX = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_))));
00152 diffY = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_))));
00153 diffZ = static_cast<unsigned char> (max (-127, min<int>(127, static_cast<int> ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_))));
00154
00155
00156 pointDiffDataVector_.push_back (diffX);
00157 pointDiffDataVector_.push_back (diffY);
00158 pointDiffDataVector_.push_back (diffZ);
00159 }
00160 }
00161
00168 void
00169 decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg,
00170 std::size_t endIdx_arg)
00171 {
00172 std::size_t i;
00173 unsigned int pointCount;
00174
00175 assert (beginIdx_arg <= endIdx_arg);
00176
00177 pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
00178
00179
00180 for (i = 0; i < pointCount; i++)
00181 {
00182
00183 const unsigned char& diffX = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
00184 const unsigned char& diffY = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
00185 const unsigned char& diffZ = static_cast<unsigned char> (*(pointDiffDataVectorIterator_++));
00186
00187
00188 PointT& point = outputCloud_arg->points[beginIdx_arg + i];
00189
00190
00191 point.x = static_cast<float> (referencePoint_arg[0] + diffX * pointCompressionResolution_);
00192 point.y = static_cast<float> (referencePoint_arg[1] + diffY * pointCompressionResolution_);
00193 point.z = static_cast<float> (referencePoint_arg[2] + diffZ * pointCompressionResolution_);
00194 }
00195 }
00196
00197 protected:
00199 PointCloudPtr output_;
00200
00202 std::vector<char> pointDiffDataVector_;
00203
00205 std::vector<char>::const_iterator pointDiffDataVectorIterator_;
00206
00208 float pointCompressionResolution_;
00209 };
00210 }
00211 }
00212
00213 #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>;
00214
00215 #endif