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 OCTREE_COMPRESSION_H
00039 #define OCTREE_COMPRESSION_H
00040
00041 #include <pcl/common/common.h>
00042 #include <pcl/common/io.h>
00043 #include <pcl/octree/octree_pointcloud.h>
00044 #include "entropy_range_coder.h"
00045 #include "color_coding.h"
00046 #include "point_coding.h"
00047
00048 #include "compression_profiles.h"
00049
00050 #include <iterator>
00051 #include <iostream>
00052 #include <vector>
00053 #include <string.h>
00054 #include <iostream>
00055 #include <stdio.h>
00056 #include <string.h>
00057
00058 namespace pcl
00059 {
00060 namespace octree
00061 {
00068 template<typename PointT, typename LeafT = OctreeContainerDataTVector<int>,
00069 typename BranchT = OctreeContainerEmpty<int>,
00070 typename OctreeT = Octree2BufBase<int, LeafT, BranchT> >
00071 class PointCloudCompression : public OctreePointCloud<PointT, LeafT,
00072 BranchT, OctreeT>
00073 {
00074 public:
00075
00076 typedef typename OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::PointCloud PointCloud;
00077 typedef typename OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::PointCloudPtr PointCloudPtr;
00078 typedef typename OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::PointCloudConstPtr PointCloudConstPtr;
00079
00080 typedef typename OctreeT::LeafNode LeafNode;
00081 typedef typename OctreeT::BranchNode BranchNode;
00082
00083 typedef PointCloudCompression<PointT, LeafT, BranchT, Octree2BufBase<int, LeafT, BranchT> > RealTimeStreamCompression;
00084 typedef PointCloudCompression<PointT, LeafT, BranchT, OctreeBase<int, LeafT, BranchT> > SinglePointCloudCompressionLowMemory;
00085
00086
00097 PointCloudCompression (compression_Profiles_e compressionProfile_arg = MED_RES_ONLINE_COMPRESSION_WITH_COLOR,
00098 bool showStatistics_arg = false,
00099 const double pointResolution_arg = 0.001,
00100 const double octreeResolution_arg = 0.01,
00101 bool doVoxelGridDownDownSampling_arg = false,
00102 const unsigned int iFrameRate_arg = 30,
00103 bool doColorEncoding_arg = true,
00104 const unsigned char colorBitResolution_arg = 6) :
00105 OctreePointCloud<PointT, LeafT, BranchT, OctreeT> (octreeResolution_arg),
00106 output_ (PointCloudPtr ()),
00107 binaryTreeDataVector_ (),
00108 binaryColorTreeVector_ (),
00109 pointCountDataVector_ (),
00110 pointCountDataVectorIterator_ (),
00111 colorCoder_ (),
00112 pointCoder_ (),
00113 entropyCoder_ (),
00114 doVoxelGridEnDecoding_ (doVoxelGridDownDownSampling_arg), iFrameRate_ (iFrameRate_arg),
00115 iFrameCounter_ (0), frameID_ (0), pointCount_ (0), iFrame_ (true),
00116 doColorEncoding_ (doColorEncoding_arg), cloudWithColor_ (false), dataWithColor_ (false),
00117 pointColorOffset_ (0), bShowStatistics (showStatistics_arg),
00118 compressedPointDataLen_ (), compressedColorDataLen_ (), selectedProfile_(compressionProfile_arg),
00119 pointResolution_(pointResolution_arg), octreeResolution_(octreeResolution_arg), colorBitResolution_(colorBitResolution_arg)
00120 {
00121 initialization();
00122 }
00123
00125 virtual
00126 ~PointCloudCompression ()
00127 {
00128 }
00129
00131 void initialization () {
00132 if (selectedProfile_ != MANUAL_CONFIGURATION)
00133 {
00134
00135
00136
00137 const configurationProfile_t selectedProfile = compressionProfiles_[selectedProfile_];
00138
00139
00140 iFrameRate_ = selectedProfile.iFrameRate;
00141 doVoxelGridEnDecoding_ = selectedProfile.doVoxelGridDownSampling;
00142 this->setResolution (selectedProfile.octreeResolution);
00143 pointCoder_.setPrecision (static_cast<float> (selectedProfile.pointResolution));
00144 doColorEncoding_ = selectedProfile.doColorEncoding;
00145 colorCoder_.setBitDepth (selectedProfile.colorBitResolution);
00146
00147 }
00148 else
00149 {
00150
00151 pointCoder_.setPrecision (static_cast<float> (pointResolution_));
00152 colorCoder_.setBitDepth (colorBitResolution_);
00153 }
00154
00155 if (pointCoder_.getPrecision () == this->getResolution ())
00156
00157 doVoxelGridEnDecoding_ = true;
00158
00159 }
00160
00164 inline void
00165 setOutputCloud (const PointCloudPtr &cloud_arg)
00166 {
00167 if (output_ != cloud_arg)
00168 {
00169 output_ = cloud_arg;
00170 }
00171 }
00172
00176 inline PointCloudPtr
00177 getOutputCloud () const
00178 {
00179 return (output_);
00180 }
00181
00186 void
00187 encodePointCloud (const PointCloudConstPtr &cloud_arg, std::ostream& compressedTreeDataOut_arg);
00188
00193 void
00194 decodePointCloud (std::istream& compressedTreeDataIn_arg, PointCloudPtr &cloud_arg);
00195
00196 protected:
00197
00201 void
00202 writeFrameHeader (std::ostream& compressedTreeDataOut_arg);
00203
00207 void
00208 readFrameHeader (std::istream& compressedTreeDataIn_arg);
00209
00213 void
00214 syncToHeader (std::istream& compressedTreeDataIn_arg);
00215
00219 void
00220 entropyEncoding (std::ostream& compressedTreeDataOut_arg);
00221
00225 void
00226 entropyDecoding (std::istream& compressedTreeDataIn_arg);
00227
00232 virtual void
00233 serializeTreeCallback (LeafNode &leaf_arg, const OctreeKey& key_arg);
00234
00239 virtual void
00240 deserializeTreeCallback (LeafNode&, const OctreeKey& key_arg);
00241
00242
00244 PointCloudPtr output_;
00245
00247 std::vector<char> binaryTreeDataVector_;
00248
00250 std::vector<char> binaryColorTreeVector_;
00251
00253 std::vector<unsigned int> pointCountDataVector_;
00254
00256 std::vector<unsigned int>::const_iterator pointCountDataVectorIterator_;
00257
00259 ColorCoding<PointT> colorCoder_;
00260
00262 PointCoding<PointT> pointCoder_;
00263
00265 StaticRangeCoder entropyCoder_;
00266
00267 bool doVoxelGridEnDecoding_;
00268 uint32_t iFrameRate_;
00269 uint32_t iFrameCounter_;
00270 uint32_t frameID_;
00271 uint64_t pointCount_;
00272 bool iFrame_;
00273
00274 bool doColorEncoding_;
00275 bool cloudWithColor_;
00276 bool dataWithColor_;
00277 unsigned char pointColorOffset_;
00278
00279
00280 bool bShowStatistics;
00281 uint64_t compressedPointDataLen_;
00282 uint64_t compressedColorDataLen_;
00283
00284
00285 static const char* frameHeaderIdentifier_;
00286
00287 const compression_Profiles_e selectedProfile_;
00288 const double pointResolution_;
00289 const double octreeResolution_;
00290 const unsigned char colorBitResolution_;
00291
00292 };
00293
00294
00295 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT>
00296 const char* PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::frameHeaderIdentifier_ = "<PCL-COMPRESSED>";
00297 }
00298
00299 }
00300
00301
00302 #endif
00303