Point Cloud Library (PCL)  1.3.1
octree_pointcloud_compression.hpp
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 OCTREE_COMPRESSION_HPP
00038 #define OCTREE_COMPRESSION_HPP
00039 
00040 #include "pcl/octree/octree_pointcloud.h"
00041 #include "pcl/compression/entropy_range_coder.h"
00042 
00043 #include <iterator>
00044 #include <iostream>
00045 #include <vector>
00046 #include <string.h>
00047 #include <iostream>
00048 #include <stdio.h>
00049 
00050 
00051 namespace pcl
00052 {
00053   namespace octree
00054   {
00055     using namespace std;
00056 
00058     template<typename PointT, typename LeafT, typename OctreeT>
00059       void
00060       PointCloudCompression<PointT, LeafT, OctreeT>::encodePointCloud (const PointCloudConstPtr &cloud_arg,
00061                                                                        std::ostream& compressedTreeDataOut_arg)
00062       {
00063         unsigned char recentTreeDepth;
00064         recentTreeDepth = this->getTreeDepth ();
00065 
00066         // initialize octree
00067         this->switchBuffers ();
00068         this->setInputCloud (cloud_arg);
00069 
00070         // color field analysis
00071         cloudWithColor_ = false;
00072         std::vector<sensor_msgs::PointField> fields;
00073         int rgba_index = -1;
00074         rgba_index = pcl::getFieldIndex (*this->input_, "rgb", fields);
00075         if (rgba_index == -1)
00076         {
00077           rgba_index = pcl::getFieldIndex (*this->input_, "rgba", fields);
00078         }
00079         if (rgba_index >= 0)
00080         {
00081           pointColorOffset_ = fields[rgba_index].offset;
00082           cloudWithColor_ = true;
00083         }
00084 
00085         // apply encoding configuration
00086         cloudWithColor_ &= doColorEncoding_;
00087 
00088         // add point to octree
00089         this->addPointsFromInputCloud ();
00090 
00091         // if octree depth changed, we enforce I-frame encoding
00092         iFrame_ = (recentTreeDepth != this->getTreeDepth ());// | !(iFrameCounter%10);
00093 
00094         // enable I-frame rate
00095         if (iFrameCounter_++==iFrameRate_) {
00096           iFrameCounter_ =0;
00097           iFrame_ = true;
00098         }
00099 
00100         // increase frameID
00101         frameID_++;
00102 
00103         // do octree encoding
00104         if (!doVoxelGridEnDecoding_)
00105         {
00106           pointCountDataVector_.clear ();
00107           pointCountDataVector_.reserve (cloud_arg->points.size ());
00108         }
00109 
00110         // initialize color encoding
00111         colorCoder_.initializeEncoding ();
00112         colorCoder_.setPointCount (cloud_arg->points.size ());
00113         colorCoder_.setVoxelCount (this->leafCount_);
00114 
00115         // initialize point encoding
00116         pointCoder_.initializeEncoding ();
00117         pointCoder_.setPointCount (cloud_arg->points.size ());
00118 
00119         // serialize octree
00120         if (iFrame_) {
00121           // i-frame encoding - encode tree structure without referencing previous buffer
00122           this->serializeTree (binaryTreeDataVector_, false);
00123         } else {
00124           // p-frame encoding - XOR encoded tree structure
00125           this->serializeTree (binaryTreeDataVector_, true);
00126         }
00127 
00128         // write frame header information to stream
00129         this->writeFrameHeader (compressedTreeDataOut_arg);
00130 
00131         // apply entropy coding to the content of all data vectors and send data to output stream
00132         this->entropyEncoding (compressedTreeDataOut_arg);
00133 
00134         if (bShowStatistics)
00135         {
00136           float bytesPerXYZ;
00137           float bytesPerColor;
00138 
00139           bytesPerXYZ = (float)compressedPointDataLen_ / (float)pointCount_;
00140           bytesPerColor = (float)compressedColorDataLen_ / (float)pointCount_;
00141 
00142 
00143           std::cerr << "*** POINTCLOUD ENCODING ***" << std::endl;
00144           std::cerr << "Frame ID: " << frameID_ << std::endl;
00145           if (iFrame_)
00146           {
00147             std::cerr << "Encoding Frame: Intra frame" << std::endl;
00148           }
00149           else
00150           {
00151             std::cerr << "Encoding Frame: Prediction frame" << std::endl;
00152           }
00153           std::cerr << "Number of encoded points: " << pointCount_ << std::endl;
00154           std::cerr << "XYZ compression percentage: " << bytesPerXYZ / (3.0f * sizeof(float)) * 100.0f
00155               << "%" << std::endl;
00156           std::cerr << "XYZ bytes per point: " << bytesPerXYZ << " bytes" << std::endl;
00157           std::cerr << "Color compression percentage: " << bytesPerColor / (sizeof(int)) * 100.0f << "%" << std::endl;
00158           std::cerr << "Color bytes per point: " << bytesPerColor << " bytes" << std::endl;
00159           std::cerr << "Size of uncompressed point cloud: " <<
00160               pointCount_* (sizeof(int) + 3.0f  * sizeof(float))  / (1024) << " kBytes" << std::endl;
00161           std::cerr << "Size of compressed point cloud: " <<
00162               (compressedPointDataLen_ + compressedColorDataLen_) / (1024) << " kBytes" << std::endl;
00163           std::cerr << "Total bytes per point: " << bytesPerXYZ + bytesPerColor << " bytes" << std::endl;
00164           std::cerr << "Total compression percentage: " << (bytesPerXYZ + bytesPerColor) / (sizeof(int) + 3.0f
00165               * sizeof(float)) * 100.0f << "%" << std::endl;
00166           std::cerr << "Compression ratio: " << (float)(sizeof(int) + 3.0f  * sizeof(float))
00167               / (float)(bytesPerXYZ + bytesPerColor)  << std::endl << std::endl;
00168         }
00169 
00170       }
00171 
00173     template<typename PointT, typename LeafT, typename OctreeT>
00174       void
00175       PointCloudCompression<PointT, LeafT, OctreeT>::decodePointCloud (std::istream& compressedTreeDataIn_arg,
00176                                                              PointCloudPtr &cloud_arg)
00177       {
00178 
00179         // initialize octree
00180         this->switchBuffers ();
00181         this->setOutputCloud (cloud_arg);
00182 
00183         // color field analysis
00184         cloudWithColor_ = false;
00185         std::vector<sensor_msgs::PointField> fields;
00186         int rgba_index = -1;
00187         rgba_index = pcl::getFieldIndex (*output_, "rgb", fields);
00188         if (rgba_index == -1)
00189         {
00190           rgba_index = pcl::getFieldIndex (*output_, "rgba", fields);
00191         }
00192         if (rgba_index >= 0)
00193         {
00194           pointColorOffset_ = fields[rgba_index].offset;
00195           cloudWithColor_ = true;
00196         }
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 (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 = cloud_arg->points.size ();
00223         output_->is_dense = false;
00224 
00225         if (bShowStatistics)
00226         {
00227           float bytesPerXYZ;
00228           float bytesPerColor;
00229 
00230           bytesPerXYZ = (float)compressedPointDataLen_ / (float)pointCount_;
00231           bytesPerColor = (float)compressedColorDataLen_ / (float)pointCount_;
00232 
00233           std::cerr << "*** POINTCLOUD DECODING ***" << std::endl;
00234           std::cerr << "Frame ID: " << frameID_ << std::endl;
00235           if (iFrame_)
00236             std::cerr << "Encoding Frame: Intra frame" << std::endl;
00237           else
00238             std::cerr << "Encoding Frame: Prediction frame" << std::endl;
00239           std::cerr << "Number of encoded points: " << pointCount_ << std::endl;
00240           std::cerr << "XYZ compression percentage: " << bytesPerXYZ / (3.0f * sizeof(float)) * 100.0f
00241                     << "%" << std::endl;
00242           std::cerr << "XYZ bytes per point: " << bytesPerXYZ << " bytes" << std::endl;
00243           std::cerr << "Color compression percentage: " << bytesPerColor / (sizeof(int)) * 100.0f 
00244                     << "%" << std::endl;
00245           std::cerr << "Color bytes per point: " << bytesPerColor << " bytes" << std::endl;
00246           std::cerr << "Size of uncompressed point cloud: " 
00247                     << pointCount_* (sizeof(int) + 3.0f  * sizeof(float))  / (1024) << " kBytes" << std::endl;
00248           std::cerr << "Size of compressed point cloud: " 
00249                     << (compressedPointDataLen_ + compressedColorDataLen_) / (1024) << " kBytes" << std::endl;
00250           std::cerr << "Total bytes per point: " << bytesPerXYZ + bytesPerColor << " bytes" << std::endl;
00251           std::cerr << "Total compression percentage: " 
00252                     << (bytesPerXYZ + bytesPerColor) / (sizeof(int) + 3.0f * sizeof(float)) * 100.0f 
00253                     << "%" << std::endl;
00254           std::cerr << "Compression ratio: " 
00255                     << (float)(sizeof(int) + 3.0f  * sizeof(float)) / (float)(bytesPerXYZ + bytesPerColor) 
00256                     << std::endl << std::endl;
00257         }
00258       }
00259 
00261     template<typename PointT, typename LeafT, typename OctreeT>
00262       void
00263       PointCloudCompression<PointT, LeafT, OctreeT>::entropyEncoding (std::ostream& compressedTreeDataOut_arg)
00264       {
00265         unsigned long binaryTreeDataVector_size;
00266         unsigned long pointAvgColorDataVector_size;
00267 
00268         compressedPointDataLen_ = 0;
00269         compressedColorDataLen_ = 0;
00270 
00271         // encode binary octree structure
00272         binaryTreeDataVector_size = binaryTreeDataVector_.size ();
00273         compressedTreeDataOut_arg.write ((const char*)&binaryTreeDataVector_size, sizeof(binaryTreeDataVector_size));
00274         compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (binaryTreeDataVector_,
00275                                                                            compressedTreeDataOut_arg);
00276 
00277         if (cloudWithColor_)
00278         {
00279           // encode averaged voxel color information
00280           std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
00281           pointAvgColorDataVector_size = pointAvgColorDataVector.size ();
00282           compressedTreeDataOut_arg.write ((const char*)&pointAvgColorDataVector_size,
00283                                            sizeof(pointAvgColorDataVector_size));
00284           compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointAvgColorDataVector,
00285                                                                              compressedTreeDataOut_arg);
00286         }
00287 
00288 
00289         if (!doVoxelGridEnDecoding_)
00290         {
00291           unsigned long pointCountDataVector_size;
00292           unsigned long pointDiffDataVector_size;
00293           unsigned long pointDiffColorDataVector_size;
00294 
00295           // encode amount of points per voxel
00296           pointCountDataVector_size = pointCountDataVector_.size ();
00297           compressedTreeDataOut_arg.write ((const char*)&pointCountDataVector_size, sizeof(pointCountDataVector_size));
00298           compressedPointDataLen_ += entropyCoder_.encodeIntVectorToStream (pointCountDataVector_,
00299                                                                             compressedTreeDataOut_arg);
00300 
00301           // encode differential point information
00302           std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
00303           pointDiffDataVector_size = pointDiffDataVector.size ();
00304           compressedTreeDataOut_arg.write ((const char*)&pointDiffDataVector_size, sizeof(pointDiffDataVector_size));
00305           compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffDataVector,
00306                                                                              compressedTreeDataOut_arg);
00307           if (cloudWithColor_)
00308           {
00309             // encode differential color information
00310             std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
00311             pointDiffColorDataVector_size = pointDiffColorDataVector.size ();
00312             compressedTreeDataOut_arg.write ((const char*)&pointDiffColorDataVector_size,
00313                                              sizeof(pointDiffColorDataVector_size));
00314             compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffColorDataVector,
00315                                                                                compressedTreeDataOut_arg);
00316           }
00317 
00318         }
00319 
00320         // flush output stream
00321         compressedTreeDataOut_arg.flush ();
00322       }
00323 
00325     template<typename PointT, typename LeafT, typename OctreeT>
00326       void
00327       PointCloudCompression<PointT, LeafT, OctreeT>::entropyDecoding (std::istream& compressedTreeDataIn_arg)
00328       {
00329         unsigned long binaryTreeDataVector_size;
00330         unsigned long pointAvgColorDataVector_size;
00331 
00332         compressedPointDataLen_ = 0;
00333         compressedColorDataLen_ = 0;
00334 
00335         // decode binary octree structure
00336         compressedTreeDataIn_arg.read ((char*)&binaryTreeDataVector_size, sizeof(binaryTreeDataVector_size));
00337         binaryTreeDataVector_.resize (binaryTreeDataVector_size);
00338         compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00339                                                                            binaryTreeDataVector_);
00340 
00341         if (dataWithColor_)
00342         {
00343           // decode averaged voxel color information
00344           std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
00345           compressedTreeDataIn_arg.read ((char*)&pointAvgColorDataVector_size, sizeof(pointAvgColorDataVector_size));
00346           pointAvgColorDataVector.resize (pointAvgColorDataVector_size);
00347           compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00348                                                                              pointAvgColorDataVector);
00349         }
00350 
00351         if (!doVoxelGridEnDecoding_)
00352         {
00353           unsigned long pointCountDataVector_size;
00354           unsigned long pointDiffDataVector_size;
00355           unsigned long pointDiffColorDataVector_size;
00356 
00357           // decode amount of points per voxel
00358           compressedTreeDataIn_arg.read ((char*)&pointCountDataVector_size, sizeof(pointCountDataVector_size));
00359           pointCountDataVector_.resize (pointCountDataVector_size);
00360           compressedPointDataLen_ += entropyCoder_.decodeStreamToIntVector (compressedTreeDataIn_arg, pointCountDataVector_);
00361           pointCountDataVectorIterator_ = pointCountDataVector_.begin ();
00362 
00363           // decode differential point information
00364           std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
00365           compressedTreeDataIn_arg.read ((char*)&pointDiffDataVector_size, sizeof(pointDiffDataVector_size));
00366           pointDiffDataVector.resize (pointDiffDataVector_size);
00367           compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00368                                                                              pointDiffDataVector);
00369 
00370           if (dataWithColor_)
00371           {
00372             // decode differential color information
00373             std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
00374             compressedTreeDataIn_arg.read ((char*)&pointDiffColorDataVector_size, sizeof(pointDiffColorDataVector_size));
00375             pointDiffColorDataVector.resize (pointDiffColorDataVector_size);
00376             compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00377                                                                                pointDiffColorDataVector);
00378           }
00379 
00380         }
00381 
00382       }
00383 
00385     template<typename PointT, typename LeafT, typename OctreeT>
00386       void
00387       PointCloudCompression<PointT, LeafT, OctreeT>::writeFrameHeader (std::ostream& compressedTreeDataOut_arg)
00388       {
00389 
00390         // encode header identifier
00391         compressedTreeDataOut_arg.write ((const char*)frameHeaderIdentifier_, strlen(frameHeaderIdentifier_));
00392 
00393         // encode point cloud header id
00394         compressedTreeDataOut_arg.write ((const char*)&frameID_, sizeof(frameID_));
00395 
00396         // encode frame type (I/P-frame)
00397         compressedTreeDataOut_arg.write ((const char*)&iFrame_, sizeof(iFrame_));
00398         if (iFrame_)
00399         {
00400           double minX, minY, minZ, maxX, maxY, maxZ;
00401           double octreeResolution;
00402           unsigned char colorBitDepth;
00403           double pointResolution;
00404 
00405           // get current configuration
00406           octreeResolution = this->getResolution();
00407           colorBitDepth  = colorCoder_.getBitDepth();
00408           pointResolution= pointCoder_.getPrecision();
00409           this->getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00410 
00411           // encode amount of points
00412           if (doVoxelGridEnDecoding_) {
00413             pointCount_ = this->leafCount_;
00414           } else
00415           {
00416             pointCount_ = this->objectCount_;
00417           }
00418 
00419           // encode coding configuration
00420           compressedTreeDataOut_arg.write ((const char*)&doVoxelGridEnDecoding_, sizeof(doVoxelGridEnDecoding_));
00421           compressedTreeDataOut_arg.write ((const char*)&cloudWithColor_, sizeof(cloudWithColor_));
00422           compressedTreeDataOut_arg.write ((const char*)&pointCount_, sizeof(pointCount_));
00423           compressedTreeDataOut_arg.write ((const char*)&octreeResolution, sizeof(octreeResolution));
00424           compressedTreeDataOut_arg.write ((const char*)&colorBitDepth, sizeof(colorBitDepth));
00425           compressedTreeDataOut_arg.write ((const char*)&pointResolution, sizeof(pointResolution));
00426 
00427           // encode octree bounding box
00428           compressedTreeDataOut_arg.write ((const char*)&minX, sizeof(minX));
00429           compressedTreeDataOut_arg.write ((const char*)&minY, sizeof(minY));
00430           compressedTreeDataOut_arg.write ((const char*)&minZ, sizeof(minZ));
00431           compressedTreeDataOut_arg.write ((const char*)&maxX, sizeof(maxX));
00432           compressedTreeDataOut_arg.write ((const char*)&maxY, sizeof(maxY));
00433           compressedTreeDataOut_arg.write ((const char*)&maxZ, sizeof(maxZ));
00434 
00435         }
00436       }
00437 
00439     template<typename PointT, typename LeafT, typename OctreeT>
00440       void
00441       PointCloudCompression<PointT, LeafT, OctreeT>::readFrameHeader ( std::istream& compressedTreeDataIn_arg)
00442       {
00443 
00444       // sync to frame header
00445         unsigned int headerIdPos = 0;
00446         while (headerIdPos < strlen(frameHeaderIdentifier_))
00447         {
00448           char readChar;
00449           compressedTreeDataIn_arg.read ((char*)&readChar, sizeof(readChar));
00450           if (readChar != frameHeaderIdentifier_[headerIdPos++])
00451           {
00452             headerIdPos = (frameHeaderIdentifier_[0]==readChar)?1:0;
00453           }
00454         }
00455 
00456         // read header
00457         compressedTreeDataIn_arg.read ((char*)&frameID_, sizeof(frameID_));
00458 
00459         compressedTreeDataIn_arg.read ((char*)&iFrame_, sizeof(iFrame_));
00460         if (iFrame_)
00461         {
00462           double minX, minY, minZ, maxX, maxY, maxZ;
00463           double octreeResolution;
00464           unsigned char colorBitDepth;
00465           double pointResolution;
00466 
00467           // read coder configuration
00468           compressedTreeDataIn_arg.read ((char*)&doVoxelGridEnDecoding_, sizeof(doVoxelGridEnDecoding_));
00469           compressedTreeDataIn_arg.read ((char*)&dataWithColor_, sizeof(dataWithColor_));
00470           compressedTreeDataIn_arg.read ((char*)&pointCount_, sizeof(pointCount_));
00471           compressedTreeDataIn_arg.read ((char*)&octreeResolution, sizeof(octreeResolution));
00472           compressedTreeDataIn_arg.read ((char*)&colorBitDepth, sizeof(colorBitDepth));
00473           compressedTreeDataIn_arg.read ((char*)&pointResolution, sizeof(pointResolution));
00474 
00475           // read octree bounding box
00476           compressedTreeDataIn_arg.read ((char*)&minX, sizeof(minX));
00477           compressedTreeDataIn_arg.read ((char*)&minY, sizeof(minY));
00478           compressedTreeDataIn_arg.read ((char*)&minZ, sizeof(minZ));
00479           compressedTreeDataIn_arg.read ((char*)&maxX, sizeof(maxX));
00480           compressedTreeDataIn_arg.read ((char*)&maxY, sizeof(maxY));
00481           compressedTreeDataIn_arg.read ((char*)&maxZ, sizeof(maxZ));
00482 
00483           // reset octree and assign new bounding box & resolution
00484           this->deleteTree ();
00485           this->setResolution (octreeResolution);
00486           this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00487 
00488           // configure color & point coding
00489           colorCoder_.setBitDepth(colorBitDepth);
00490           pointCoder_.setPrecision(pointResolution);
00491 
00492         }
00493 
00494       }
00495 
00497     template<typename PointT, typename LeafT, typename OctreeT>
00498       void
00499       PointCloudCompression<PointT, LeafT, OctreeT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
00500       {
00501 
00502       // reference to point indices vector stored within octree leaf
00503       const std::vector<int>& leafIdx = leaf_arg.getIdxVector ();
00504 
00505         if (!doVoxelGridEnDecoding_)
00506         {
00507           double lowerVoxelCorner[3];
00508 
00509           // encode amount of points within voxel
00510           pointCountDataVector_.push_back ((int)leafIdx.size ());
00511 
00512           // calculate lower voxel corner based on octree key
00513           lowerVoxelCorner[0] = ((double)key_arg.x) * this->resolution_ + this->minX_;
00514           lowerVoxelCorner[1] = ((double)key_arg.y) * this->resolution_ + this->minY_;
00515           lowerVoxelCorner[2] = ((double)key_arg.z) * this->resolution_ + this->minZ_;
00516 
00517           // differentially encode points to lower voxel corner
00518           pointCoder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
00519 
00520           if (cloudWithColor_)
00521           {
00522             // encode color of points
00523             colorCoder_.encodePoints (leafIdx, pointColorOffset_, this->input_);
00524           }
00525 
00526         }
00527         else
00528         {
00529           if (cloudWithColor_)
00530           {
00531             // encode average color of all points within voxel
00532             colorCoder_.encodeAverageOfPoints (leafIdx, pointColorOffset_, this->input_);
00533           }
00534         }
00535 
00536       }
00537 
00539     template<typename PointT, typename LeafT, typename OctreeT>
00540       void
00541       PointCloudCompression<PointT, LeafT, OctreeT>::deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
00542       {
00543         double lowerVoxelCorner[3];
00544         std::size_t pointCount, i, cloudSize;
00545         PointT newPoint;
00546 
00547         pointCount = 1;
00548 
00549         if (!doVoxelGridEnDecoding_)
00550         {
00551           // get current cloud size
00552           cloudSize = this->output_->points.size ();
00553 
00554           // get amount of point to be decoded
00555           pointCount = *pointCountDataVectorIterator_;
00556           pointCountDataVectorIterator_++;
00557 
00558           // increase point cloud by amount of voxel points
00559           for (i = 0; i < pointCount; i++)
00560           {
00561             this->output_->points.push_back (newPoint);
00562           }
00563 
00564           // calculcate position of lower voxel corner
00565           lowerVoxelCorner[0] = ((double)key_arg.x) * this->resolution_ + this->minX_;
00566           lowerVoxelCorner[1] = ((double)key_arg.y) * this->resolution_ + this->minY_;
00567           lowerVoxelCorner[2] = ((double)key_arg.z) * this->resolution_ + this->minZ_;
00568 
00569           // decode differentially encoded points
00570           pointCoder_.decodePoints (this->output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount);
00571 
00572         }
00573         else
00574         {
00575           // calculcate center of lower voxel corner
00576           newPoint.x = ((double)key_arg.x + 0.5) * this->resolution_ + this->minX_;
00577           newPoint.y = ((double)key_arg.y + 0.5) * this->resolution_ + this->minY_;
00578           newPoint.z = ((double)key_arg.z + 0.5) * this->resolution_ + this->minZ_;
00579 
00580           // add point to point cloud
00581           this->output_->points.push_back (newPoint);
00582 
00583         }
00584 
00585         if (cloudWithColor_)
00586         {
00587           if (dataWithColor_)
00588           {
00589             // decode color information
00590             colorCoder_.decodePoints (this->output_, this->output_->points.size () - pointCount,
00591                                       this->output_->points.size (), pointColorOffset_);
00592           }
00593           else
00594           {
00595             // set default color information
00596             colorCoder_.setDefaultColor (this->output_, this->output_->points.size () - pointCount,
00597                                          this->output_->points.size (), pointColorOffset_);
00598           }
00599         }
00600 
00601       }
00602 
00603   }
00604   ;
00605 }
00606 
00607 #define PCL_INSTANTIATE_PointCloudCompression(T) template class PCL_EXPORTS pcl::octree::PointCloudCompression<T>;
00608 
00609 #endif
00610 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines