octree_pointcloud_compression.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, Willow Garage, Inc.
00006  * 
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00038 #ifndef OCTREE_COMPRESSION_HPP
00039 #define OCTREE_COMPRESSION_HPP
00040 
00041 #include <pcl/octree/octree_pointcloud.h>
00042 #include <pcl/compression/entropy_range_coder.h>
00043 
00044 #include <iterator>
00045 #include <iostream>
00046 #include <vector>
00047 #include <string.h>
00048 #include <iostream>
00049 #include <stdio.h>
00050 
00051 
00052 namespace pcl
00053 {
00054   namespace octree
00055   {
00057     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void PointCloudCompression<
00058         PointT, LeafT, BranchT, OctreeT>::encodePointCloud (
00059         const PointCloudConstPtr &cloud_arg,
00060         std::ostream& compressedTreeDataOut_arg)
00061     {
00062       unsigned char recentTreeDepth =
00063           static_cast<unsigned char> (this->getTreeDepth ());
00064 
00065       // initialize octree
00066       this->setInputCloud (cloud_arg);
00067 
00068       // add point to octree
00069       this->addPointsFromInputCloud ();
00070 
00071       // make sure cloud contains points
00072       if (this->leafCount_>0) {
00073 
00074 
00075         // color field analysis
00076         cloudWithColor_ = false;
00077         std::vector<sensor_msgs::PointField> fields;
00078         int rgba_index = -1;
00079         rgba_index = pcl::getFieldIndex (*this->input_, "rgb", fields);
00080         if (rgba_index == -1)
00081         {
00082           rgba_index = pcl::getFieldIndex (*this->input_, "rgba", fields);
00083         }
00084         if (rgba_index >= 0)
00085         {
00086           pointColorOffset_ = static_cast<unsigned char> (fields[rgba_index].offset);
00087           cloudWithColor_ = true;
00088         }
00089 
00090         // apply encoding configuration
00091         cloudWithColor_ &= doColorEncoding_;
00092 
00093 
00094         // if octree depth changed, we enforce I-frame encoding
00095         iFrame_ |= (recentTreeDepth != this->getTreeDepth ());// | !(iFrameCounter%10);
00096 
00097         // enable I-frame rate
00098         if (iFrameCounter_++==iFrameRate_)
00099         {
00100           iFrameCounter_ =0;
00101           iFrame_ = true;
00102         }
00103 
00104         // increase frameID
00105         frameID_++;
00106 
00107         // do octree encoding
00108         if (!doVoxelGridEnDecoding_)
00109         {
00110           pointCountDataVector_.clear ();
00111           pointCountDataVector_.reserve (cloud_arg->points.size ());
00112         }
00113 
00114         // initialize color encoding
00115         colorCoder_.initializeEncoding ();
00116         colorCoder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
00117         colorCoder_.setVoxelCount (static_cast<unsigned int> (this->leafCount_));
00118 
00119         // initialize point encoding
00120         pointCoder_.initializeEncoding ();
00121         pointCoder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
00122 
00123         // serialize octree
00124         if (iFrame_)
00125           // i-frame encoding - encode tree structure without referencing previous buffer
00126           this->serializeTree (binaryTreeDataVector_, false);
00127         else
00128           // p-frame encoding - XOR encoded tree structure
00129           this->serializeTree (binaryTreeDataVector_, true);
00130 
00131         // write frame header information to stream
00132         this->writeFrameHeader (compressedTreeDataOut_arg);
00133 
00134         // apply entropy coding to the content of all data vectors and send data to output stream
00135         this->entropyEncoding (compressedTreeDataOut_arg);
00136 
00137         // prepare for next frame
00138         this->switchBuffers ();
00139         iFrame_ = false;
00140 
00141         if (bShowStatistics)
00142         {
00143           float bytesPerXYZ = static_cast<float> (compressedPointDataLen_) / static_cast<float> (pointCount_);
00144           float bytesPerColor = static_cast<float> (compressedColorDataLen_) / static_cast<float> (pointCount_);
00145 
00146           PCL_INFO ("*** POINTCLOUD ENCODING ***\n");
00147           PCL_INFO ("Frame ID: %d\n", frameID_);
00148           if (iFrame_)
00149             PCL_INFO ("Encoding Frame: Intra frame\n");
00150           else
00151             PCL_INFO ("Encoding Frame: Prediction frame\n");
00152           PCL_INFO ("Number of encoded points: %ld\n", pointCount_);
00153           PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof(float)) * 100.0f);
00154           PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ);
00155           PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f);
00156           PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor);
00157           PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (pointCount_) * (sizeof (int) + 3.0f  * sizeof (float)) / 1024);
00158           PCL_INFO ("Size of compressed point cloud: %d kBytes\n", (compressedPointDataLen_ + compressedColorDataLen_) / (1024));
00159           PCL_INFO ("Total bytes per point: %f\n", bytesPerXYZ + bytesPerColor);
00160           PCL_INFO ("Total compression percentage: %f\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof(float)) * 100.0f);
00161           PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytesPerXYZ + bytesPerColor));
00162         }
00163       } else {
00164         if (bShowStatistics)
00165         PCL_INFO ("Info: Dropping empty point cloud\n");
00166         this->deleteTree();
00167         iFrameCounter_ = 0;
00168         iFrame_ = true;
00169       }
00170     }
00171 
00173     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00174     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::decodePointCloud (
00175         std::istream& compressedTreeDataIn_arg,
00176         PointCloudPtr &cloud_arg)
00177     {
00178 
00179       // synchronize to frame header
00180       syncToHeader(compressedTreeDataIn_arg);
00181 
00182       // initialize octree
00183       this->switchBuffers ();
00184       this->setOutputCloud (cloud_arg);
00185 
00186       // color field analysis
00187       cloudWithColor_ = false;
00188       std::vector<sensor_msgs::PointField> fields;
00189       int rgba_index = -1;
00190       rgba_index = pcl::getFieldIndex (*output_, "rgb", fields);
00191       if (rgba_index == -1)
00192         rgba_index = pcl::getFieldIndex (*output_, "rgba", fields);
00193       if (rgba_index >= 0)
00194       {
00195         pointColorOffset_ = static_cast<unsigned char> (fields[rgba_index].offset);
00196         cloudWithColor_ = true;
00197       }
00198 
00199       // read header from input stream
00200       this->readFrameHeader (compressedTreeDataIn_arg);
00201 
00202       // decode data vectors from stream
00203       this->entropyDecoding (compressedTreeDataIn_arg);
00204 
00205       // initialize color and point encoding
00206       colorCoder_.initializeDecoding ();
00207       pointCoder_.initializeDecoding ();
00208 
00209       // initialize output cloud
00210       output_->points.clear ();
00211       output_->points.reserve (static_cast<std::size_t> (pointCount_));
00212 
00213       if (iFrame_)
00214         // i-frame decoding - decode tree structure without referencing previous buffer
00215         this->deserializeTree (binaryTreeDataVector_, false);
00216       else
00217         // p-frame decoding - decode XOR encoded tree structure
00218         this->deserializeTree (binaryTreeDataVector_, true);
00219 
00220       // assign point cloud properties
00221       output_->height = 1;
00222       output_->width = static_cast<uint32_t> (cloud_arg->points.size ());
00223       output_->is_dense = false;
00224 
00225       if (bShowStatistics)
00226       {
00227         float bytesPerXYZ = static_cast<float> (compressedPointDataLen_) / static_cast<float> (pointCount_);
00228         float bytesPerColor = static_cast<float> (compressedColorDataLen_) / static_cast<float> (pointCount_);
00229 
00230         PCL_INFO ("*** POINTCLOUD DECODING ***\n");
00231         PCL_INFO ("Frame ID: %d\n", frameID_);
00232         if (iFrame_)
00233           PCL_INFO ("Encoding Frame: Intra frame\n");
00234         else
00235           PCL_INFO ("Encoding Frame: Prediction frame\n");
00236         PCL_INFO ("Number of encoded points: %ld\n", pointCount_);
00237         PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof (float)) * 100.0f);
00238         PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ);
00239         PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f);
00240         PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor);
00241         PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f);
00242         PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast<float> (compressedPointDataLen_ + compressedColorDataLen_) / 1024.0f);
00243         PCL_INFO ("Total bytes per point: %d bytes\n", static_cast<int> (bytesPerXYZ + bytesPerColor));
00244         PCL_INFO ("Total compression percentage: %f%%\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f);
00245         PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytesPerXYZ + bytesPerColor));
00246       }
00247     }
00248 
00250     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00251     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyEncoding (std::ostream& compressedTreeDataOut_arg)
00252     {
00253       uint64_t binaryTreeDataVector_size;
00254       uint64_t pointAvgColorDataVector_size;
00255 
00256       compressedPointDataLen_ = 0;
00257       compressedColorDataLen_ = 0;
00258 
00259       // encode binary octree structure
00260       binaryTreeDataVector_size = binaryTreeDataVector_.size ();
00261       compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size));
00262       compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (binaryTreeDataVector_,
00263                                                                          compressedTreeDataOut_arg);
00264 
00265       if (cloudWithColor_)
00266       {
00267         // encode averaged voxel color information
00268         std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
00269         pointAvgColorDataVector_size = pointAvgColorDataVector.size ();
00270         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointAvgColorDataVector_size),
00271                                          sizeof (pointAvgColorDataVector_size));
00272         compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointAvgColorDataVector,
00273                                                                            compressedTreeDataOut_arg);
00274       }
00275 
00276       if (!doVoxelGridEnDecoding_)
00277       {
00278         uint64_t pointCountDataVector_size;
00279         uint64_t pointDiffDataVector_size;
00280         uint64_t pointDiffColorDataVector_size;
00281 
00282         // encode amount of points per voxel
00283         pointCountDataVector_size = pointCountDataVector_.size ();
00284         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
00285         compressedPointDataLen_ += entropyCoder_.encodeIntVectorToStream (pointCountDataVector_,
00286                                                                           compressedTreeDataOut_arg);
00287 
00288         // encode differential point information
00289         std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
00290         pointDiffDataVector_size = pointDiffDataVector.size ();
00291         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size));
00292         compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffDataVector,
00293                                                                            compressedTreeDataOut_arg);
00294         if (cloudWithColor_)
00295         {
00296           // encode differential color information
00297           std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
00298           pointDiffColorDataVector_size = pointDiffColorDataVector.size ();
00299           compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointDiffColorDataVector_size),
00300                                            sizeof (pointDiffColorDataVector_size));
00301           compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffColorDataVector,
00302                                                                              compressedTreeDataOut_arg);
00303         }
00304       }
00305       // flush output stream
00306       compressedTreeDataOut_arg.flush ();
00307     }
00308 
00310     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00311     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyDecoding (std::istream& compressedTreeDataIn_arg)
00312     {
00313       uint64_t binaryTreeDataVector_size;
00314       uint64_t pointAvgColorDataVector_size;
00315 
00316       compressedPointDataLen_ = 0;
00317       compressedColorDataLen_ = 0;
00318 
00319       // decode binary octree structure
00320       compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size));
00321       binaryTreeDataVector_.resize (static_cast<std::size_t> (binaryTreeDataVector_size));
00322       compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00323                                                                          binaryTreeDataVector_);
00324 
00325       if (dataWithColor_)
00326       {
00327         // decode averaged voxel color information
00328         std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
00329         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointAvgColorDataVector_size), sizeof (pointAvgColorDataVector_size));
00330         pointAvgColorDataVector.resize (static_cast<std::size_t> (pointAvgColorDataVector_size));
00331         compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00332                                                                            pointAvgColorDataVector);
00333       }
00334 
00335       if (!doVoxelGridEnDecoding_)
00336       {
00337         uint64_t pointCountDataVector_size;
00338         uint64_t pointDiffDataVector_size;
00339         uint64_t pointDiffColorDataVector_size;
00340 
00341         // decode amount of points per voxel
00342         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
00343         pointCountDataVector_.resize (static_cast<std::size_t> (pointCountDataVector_size));
00344         compressedPointDataLen_ += entropyCoder_.decodeStreamToIntVector (compressedTreeDataIn_arg, pointCountDataVector_);
00345         pointCountDataVectorIterator_ = pointCountDataVector_.begin ();
00346 
00347         // decode differential point information
00348         std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
00349         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size));
00350         pointDiffDataVector.resize (static_cast<std::size_t> (pointDiffDataVector_size));
00351         compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00352                                                                            pointDiffDataVector);
00353 
00354         if (dataWithColor_)
00355         {
00356           // decode differential color information
00357           std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
00358           compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointDiffColorDataVector_size), sizeof (pointDiffColorDataVector_size));
00359           pointDiffColorDataVector.resize (static_cast<std::size_t> (pointDiffColorDataVector_size));
00360           compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00361                                                                              pointDiffColorDataVector);
00362         }
00363       }
00364     }
00365 
00367     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00368     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::writeFrameHeader (std::ostream& compressedTreeDataOut_arg)
00369     {
00370       // encode header identifier
00371       compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
00372       // encode point cloud header id
00373       compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&frameID_), sizeof (frameID_));
00374       // encode frame type (I/P-frame)
00375       compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&iFrame_), sizeof (iFrame_));
00376       if (iFrame_)
00377       {
00378         double minX, minY, minZ, maxX, maxY, maxZ;
00379         double octreeResolution;
00380         unsigned char colorBitDepth;
00381         double pointResolution;
00382 
00383         // get current configuration
00384         octreeResolution = this->getResolution ();
00385         colorBitDepth  = colorCoder_.getBitDepth ();
00386         pointResolution= pointCoder_.getPrecision ();
00387         this->getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00388 
00389         // encode amount of points
00390         if (doVoxelGridEnDecoding_)
00391           pointCount_ = this->leafCount_;
00392         else
00393           pointCount_ = this->objectCount_;
00394 
00395         // encode coding configuration
00396         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_));
00397         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&cloudWithColor_), sizeof (cloudWithColor_));
00398         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointCount_), sizeof (pointCount_));
00399         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&octreeResolution), sizeof (octreeResolution));
00400         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&colorBitDepth), sizeof (colorBitDepth));
00401         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointResolution), sizeof (pointResolution));
00402 
00403         // encode octree bounding box
00404         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minX), sizeof (minX));
00405         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minY), sizeof (minY));
00406         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minZ), sizeof (minZ));
00407         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxX), sizeof (maxX));
00408         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxY), sizeof (maxY));
00409         compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxZ), sizeof (maxZ));
00410       }
00411     }
00412 
00414     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00415     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::syncToHeader ( std::istream& compressedTreeDataIn_arg)
00416     {
00417       // sync to frame header
00418       unsigned int headerIdPos = 0;
00419       while (headerIdPos < strlen (frameHeaderIdentifier_))
00420       {
00421         char readChar;
00422         compressedTreeDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
00423         if (readChar != frameHeaderIdentifier_[headerIdPos++])
00424           headerIdPos = (frameHeaderIdentifier_[0]==readChar)?1:0;
00425       }
00426     }
00427 
00429     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00430     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::readFrameHeader ( std::istream& compressedTreeDataIn_arg)
00431     {
00432       // read header
00433       compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&frameID_), sizeof (frameID_));
00434       compressedTreeDataIn_arg.read (reinterpret_cast<char*>(&iFrame_), sizeof (iFrame_));
00435       if (iFrame_)
00436       {
00437         double minX, minY, minZ, maxX, maxY, maxZ;
00438         double octreeResolution;
00439         unsigned char colorBitDepth;
00440         double pointResolution;
00441 
00442         // read coder configuration
00443         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_));
00444         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&dataWithColor_), sizeof (dataWithColor_));
00445         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointCount_), sizeof (pointCount_));
00446         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&octreeResolution), sizeof (octreeResolution));
00447         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&colorBitDepth), sizeof (colorBitDepth));
00448         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointResolution), sizeof (pointResolution));
00449 
00450         // read octree bounding box
00451         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minX), sizeof (minX));
00452         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minY), sizeof (minY));
00453         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minZ), sizeof (minZ));
00454         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxX), sizeof (maxX));
00455         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxY), sizeof (maxY));
00456         compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxZ), sizeof (maxZ));
00457 
00458         // reset octree and assign new bounding box & resolution
00459         this->deleteTree ();
00460         this->setResolution (octreeResolution);
00461         this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00462 
00463         // configure color & point coding
00464         colorCoder_.setBitDepth (colorBitDepth);
00465         pointCoder_.setPrecision (static_cast<float> (pointResolution));
00466       }
00467     }
00468 
00470     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00471     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::serializeTreeCallback (
00472         LeafNode &leaf_arg, const OctreeKey & key_arg)
00473     {
00474       // reference to point indices vector stored within octree leaf
00475       const std::vector<int>& leafIdx = leaf_arg.getDataTVector ();
00476 
00477       if (!doVoxelGridEnDecoding_)
00478       {
00479         double lowerVoxelCorner[3];
00480 
00481         // encode amount of points within voxel
00482         pointCountDataVector_.push_back (static_cast<int> (leafIdx.size ()));
00483 
00484         // calculate lower voxel corner based on octree key
00485         lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->minX_;
00486         lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->minY_;
00487         lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->minZ_;
00488 
00489         // differentially encode points to lower voxel corner
00490         pointCoder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
00491 
00492         if (cloudWithColor_)
00493           // encode color of points
00494           colorCoder_.encodePoints (leafIdx, pointColorOffset_, this->input_);
00495       }
00496       else
00497       {
00498         if (cloudWithColor_)
00499           // encode average color of all points within voxel
00500           colorCoder_.encodeAverageOfPoints (leafIdx, pointColorOffset_, this->input_);
00501       }
00502     }
00503 
00505     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00506     PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::deserializeTreeCallback (LeafNode&,
00507         const OctreeKey& key_arg)
00508     {
00509       double lowerVoxelCorner[3];
00510       std::size_t pointCount, i, cloudSize;
00511       PointT newPoint;
00512 
00513       pointCount = 1;
00514 
00515       if (!doVoxelGridEnDecoding_)
00516       {
00517         // get current cloud size
00518         cloudSize = output_->points.size ();
00519 
00520         // get amount of point to be decoded
00521         pointCount = *pointCountDataVectorIterator_;
00522         pointCountDataVectorIterator_++;
00523 
00524         // increase point cloud by amount of voxel points
00525         for (i = 0; i < pointCount; i++)
00526           output_->points.push_back (newPoint);
00527 
00528         // calculcate position of lower voxel corner
00529         lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->minX_;
00530         lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->minY_;
00531         lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->minZ_;
00532 
00533         // decode differentially encoded points
00534         pointCoder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount);
00535       }
00536       else
00537       {
00538         // calculate center of lower voxel corner
00539         newPoint.x = static_cast<float> ((static_cast<double> (key_arg.x) + 0.5) * this->resolution_ + this->minX_);
00540         newPoint.y = static_cast<float> ((static_cast<double> (key_arg.y) + 0.5) * this->resolution_ + this->minY_);
00541         newPoint.z = static_cast<float> ((static_cast<double> (key_arg.z) + 0.5) * this->resolution_ + this->minZ_);
00542 
00543         // add point to point cloud
00544         output_->points.push_back (newPoint);
00545       }
00546 
00547       if (cloudWithColor_)
00548       {
00549         if (dataWithColor_)
00550           // decode color information
00551           colorCoder_.decodePoints (output_, output_->points.size () - pointCount,
00552                                     output_->points.size (), pointColorOffset_);
00553         else
00554           // set default color information
00555           colorCoder_.setDefaultColor (output_, output_->points.size () - pointCount,
00556                                        output_->points.size (), pointColorOffset_);
00557       }
00558     }
00559   }
00560 }
00561 
00562 #define PCL_INSTANTIATE_PointCloudCompression(T) template class PCL_EXPORTS pcl::octree::PointCloudCompression<T, pcl::octree::OctreeContainerDataTVector<int>, pcl::octree::OctreeContainerEmpty<int>, pcl::octree::Octree2BufBase<int, pcl::octree::OctreeContainerDataTVector<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00563 
00564 #endif
00565 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:57