Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/io/include/pcl/compression/octree_pointcloud_compression.h
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_H
00039 #define OCTREE_COMPRESSION_H
00040 
00041 #include <pcl/common/common.h>
00042 #include <pcl/common/io.h>
00043 #include <pcl/octree/octree2buf_base.h>
00044 #include <pcl/octree/octree_pointcloud.h>
00045 #include "entropy_range_coder.h"
00046 #include "color_coding.h"
00047 #include "point_coding.h"
00048 
00049 #include "compression_profiles.h"
00050 
00051 #include <iterator>
00052 #include <iostream>
00053 #include <vector>
00054 #include <string.h>
00055 #include <iostream>
00056 #include <stdio.h>
00057 #include <string.h>
00058 
00059 using namespace pcl::octree;
00060 
00061 namespace pcl
00062 {
00063   namespace io
00064   {
00065     /** \brief @b Octree pointcloud compression class
00066      *  \note This class enables compression and decompression of point cloud data based on octree data structures.
00067      *  \note
00068      *  \note typename: PointT: type of point used in pointcloud
00069      *  \author Julius Kammerl (julius@kammerl.de)
00070      */
00071     template<typename PointT, typename LeafT = OctreeContainerPointIndices,
00072         typename BranchT = OctreeContainerEmpty,
00073         typename OctreeT = Octree2BufBase<LeafT, BranchT> >
00074     class OctreePointCloudCompression : public OctreePointCloud<PointT, LeafT,
00075         BranchT, OctreeT>
00076     {
00077       public:
00078         // public typedefs
00079         typedef typename OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::PointCloud PointCloud;
00080         typedef typename OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::PointCloudPtr PointCloudPtr;
00081         typedef typename OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::PointCloudConstPtr PointCloudConstPtr;
00082 
00083         typedef typename OctreeT::LeafNode LeafNode;
00084         typedef typename OctreeT::BranchNode BranchNode;
00085 
00086         typedef OctreePointCloudCompression<PointT, LeafT, BranchT, Octree2BufBase<LeafT, BranchT> > RealTimeStreamCompression;
00087         typedef OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeBase<LeafT, BranchT> > SinglePointCloudCompressionLowMemory;
00088 
00089 
00090         /** \brief Constructor
00091           * \param compressionProfile_arg:  define compression profile
00092           * \param octreeResolution_arg:  octree resolution at lowest octree level
00093           * \param pointResolution_arg:  precision of point coordinates
00094           * \param doVoxelGridDownDownSampling_arg:  voxel grid filtering
00095           * \param iFrameRate_arg:  i-frame encoding rate
00096           * \param doColorEncoding_arg:  enable/disable color coding
00097           * \param colorBitResolution_arg:  color bit depth
00098           * \param showStatistics_arg:  output compression statistics
00099           */
00100         OctreePointCloudCompression (compression_Profiles_e compressionProfile_arg = MED_RES_ONLINE_COMPRESSION_WITH_COLOR,
00101                                bool showStatistics_arg = false,
00102                                const double pointResolution_arg = 0.001,
00103                                const double octreeResolution_arg = 0.01,
00104                                bool doVoxelGridDownDownSampling_arg = false,
00105                                const unsigned int iFrameRate_arg = 30,
00106                                bool doColorEncoding_arg = true,
00107                                const unsigned char colorBitResolution_arg = 6) :
00108           OctreePointCloud<PointT, LeafT, BranchT, OctreeT> (octreeResolution_arg),
00109           output_ (PointCloudPtr ()),
00110           binary_tree_data_vector_ (),
00111           binary_color_tree_vector_ (),
00112           point_count_data_vector_ (),
00113           point_count_data_vector_iterator_ (),
00114           color_coder_ (),
00115           point_coder_ (),
00116           entropy_coder_ (),
00117           do_voxel_grid_enDecoding_ (doVoxelGridDownDownSampling_arg), i_frame_rate_ (iFrameRate_arg),
00118           i_frame_counter_ (0), frame_ID_ (0), point_count_ (0), i_frame_ (true),
00119           do_color_encoding_ (doColorEncoding_arg), cloud_with_color_ (false), data_with_color_ (false),
00120           point_color_offset_ (0), b_show_statistics_ (showStatistics_arg), 
00121           compressed_point_data_len_ (), compressed_color_data_len_ (), selected_profile_(compressionProfile_arg),
00122           point_resolution_(pointResolution_arg), octree_resolution_(octreeResolution_arg),
00123           color_bit_resolution_(colorBitResolution_arg),
00124           object_count_(0)
00125         {
00126           initialization();
00127         }
00128 
00129         /** \brief Empty deconstructor. */
00130         virtual
00131         ~OctreePointCloudCompression ()
00132         {
00133         }
00134 
00135         /** \brief Initialize globals */
00136         void initialization () {
00137           if (selected_profile_ != MANUAL_CONFIGURATION)
00138           {
00139             // apply selected compression profile
00140 
00141             // retrieve profile settings
00142             const configurationProfile_t selectedProfile = compressionProfiles_[selected_profile_];
00143 
00144             // apply profile settings
00145             i_frame_rate_ = selectedProfile.iFrameRate;
00146             do_voxel_grid_enDecoding_ = selectedProfile.doVoxelGridDownSampling;
00147             this->setResolution (selectedProfile.octreeResolution);
00148             point_coder_.setPrecision (static_cast<float> (selectedProfile.pointResolution));
00149             do_color_encoding_ = selectedProfile.doColorEncoding;
00150             color_coder_.setBitDepth (selectedProfile.colorBitResolution);
00151 
00152           }
00153           else 
00154           {
00155             // configure point & color coder
00156             point_coder_.setPrecision (static_cast<float> (point_resolution_));
00157             color_coder_.setBitDepth (color_bit_resolution_);
00158           }
00159 
00160           if (point_coder_.getPrecision () == this->getResolution ())
00161             //disable differential point colding
00162             do_voxel_grid_enDecoding_ = true;
00163 
00164         }
00165 
00166         /** \brief Add point at index from input pointcloud dataset to octree
00167          * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added
00168          */
00169         virtual void
00170         addPointIdx (const int pointIdx_arg)
00171         {
00172           ++object_count_;
00173           OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointIdx(pointIdx_arg);
00174         }
00175 
00176         /** \brief Provide a pointer to the output data set.
00177           * \param cloud_arg: the boost shared pointer to a PointCloud message
00178           */
00179         inline void
00180         setOutputCloud (const PointCloudPtr &cloud_arg)
00181         {
00182           if (output_ != cloud_arg)
00183           {
00184             output_ = cloud_arg;
00185           }
00186         }
00187 
00188         /** \brief Get a pointer to the output point cloud dataset.
00189           * \return pointer to pointcloud output class.
00190           */
00191         inline PointCloudPtr
00192         getOutputCloud () const
00193         {
00194           return (output_);
00195         }
00196 
00197         /** \brief Encode point cloud to output stream
00198           * \param cloud_arg:  point cloud to be compressed
00199           * \param compressed_tree_data_out_arg:  binary output stream containing compressed data
00200           */
00201         void
00202         encodePointCloud (const PointCloudConstPtr &cloud_arg, std::ostream& compressed_tree_data_out_arg);
00203 
00204         /** \brief Decode point cloud from input stream
00205           * \param compressed_tree_data_in_arg: binary input stream containing compressed data
00206           * \param cloud_arg: reference to decoded point cloud
00207           */
00208         void
00209         decodePointCloud (std::istream& compressed_tree_data_in_arg, PointCloudPtr &cloud_arg);
00210 
00211       protected:
00212 
00213         /** \brief Write frame information to output stream
00214           * \param compressed_tree_data_out_arg: binary output stream
00215           */
00216         void
00217         writeFrameHeader (std::ostream& compressed_tree_data_out_arg);
00218 
00219         /** \brief Read frame information to output stream
00220           * \param compressed_tree_data_in_arg: binary input stream
00221           */
00222         void
00223         readFrameHeader (std::istream& compressed_tree_data_in_arg);
00224 
00225         /** \brief Synchronize to frame header
00226           * \param compressed_tree_data_in_arg: binary input stream
00227           */
00228         void
00229         syncToHeader (std::istream& compressed_tree_data_in_arg);
00230 
00231         /** \brief Apply entropy encoding to encoded information and output to binary stream
00232           * \param compressed_tree_data_out_arg: binary output stream
00233           */
00234         void
00235         entropyEncoding (std::ostream& compressed_tree_data_out_arg);
00236 
00237         /** \brief Entropy decoding of input binary stream and output to information vectors
00238           * \param compressed_tree_data_in_arg: binary input stream
00239           */
00240         void
00241         entropyDecoding (std::istream& compressed_tree_data_in_arg);
00242 
00243         /** \brief Encode leaf node information during serialization
00244           * \param leaf_arg: reference to new leaf node
00245           * \param key_arg: octree key of new leaf node
00246          */
00247         virtual void
00248         serializeTreeCallback (LeafT &leaf_arg, const OctreeKey& key_arg);
00249 
00250         /** \brief Decode leaf nodes information during deserialization
00251           * \param leaf_arg: reference to new leaf node
00252          * \param key_arg: octree key of new leaf node
00253          */
00254         virtual void
00255         deserializeTreeCallback (LeafT&, const OctreeKey& key_arg);
00256 
00257 
00258         /** \brief Pointer to output point cloud dataset. */
00259         PointCloudPtr output_;
00260 
00261         /** \brief Vector for storing binary tree structure */
00262         std::vector<char> binary_tree_data_vector_;
00263 
00264         /** \brief Interator on binary tree structure vector */
00265         std::vector<char> binary_color_tree_vector_;
00266 
00267         /** \brief Vector for storing points per voxel information  */
00268         std::vector<unsigned int> point_count_data_vector_;
00269 
00270         /** \brief Interator on points per voxel vector */
00271         std::vector<unsigned int>::const_iterator point_count_data_vector_iterator_;
00272 
00273         /** \brief Color coding instance */
00274         ColorCoding<PointT> color_coder_;
00275 
00276         /** \brief Point coding instance */
00277         PointCoding<PointT> point_coder_;
00278 
00279         /** \brief Static range coder instance */
00280         StaticRangeCoder entropy_coder_;
00281 
00282         bool do_voxel_grid_enDecoding_;
00283         uint32_t i_frame_rate_;
00284         uint32_t i_frame_counter_;
00285         uint32_t frame_ID_;
00286         uint64_t point_count_;
00287         bool i_frame_;
00288 
00289         bool do_color_encoding_;
00290         bool cloud_with_color_;
00291         bool data_with_color_;
00292         unsigned char point_color_offset_;
00293 
00294         //bool activating statistics
00295         bool b_show_statistics_;
00296         uint64_t compressed_point_data_len_;
00297         uint64_t compressed_color_data_len_;
00298 
00299         // frame header identifier
00300         static const char* frame_header_identifier_;
00301 
00302         const compression_Profiles_e selected_profile_;
00303         const double point_resolution_;
00304         const double octree_resolution_;
00305         const unsigned char color_bit_resolution_;
00306 
00307         std::size_t object_count_;
00308 
00309       };
00310 
00311     // define frame identifier
00312     template<typename PointT, typename LeafT, typename BranchT, typename OctreeT>
00313       const char* OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::frame_header_identifier_ = "<PCL-OCT-COMPRESSED>";
00314   }
00315 
00316 }
00317 
00318 
00319 #endif
00320