Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
octree_pointcloud_compression.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef OCTREE_COMPRESSION_HPP
39 #define OCTREE_COMPRESSION_HPP
40 
41 #include <pcl/octree/octree_pointcloud.h>
42 #include <pcl/compression/entropy_range_coder.h>
43 
44 #include <iterator>
45 #include <iostream>
46 #include <vector>
47 #include <string.h>
48 #include <iostream>
49 #include <stdio.h>
50 
51 
52 namespace pcl
53 {
54  namespace octree
55  {
57  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void PointCloudCompression<
58  PointT, LeafT, BranchT, OctreeT>::encodePointCloud (
59  const PointCloudConstPtr &cloud_arg,
60  std::ostream& compressedTreeDataOut_arg)
61  {
62  unsigned char recentTreeDepth =
63  static_cast<unsigned char> (this->getTreeDepth ());
64 
65  // initialize octree
66  this->setInputCloud (cloud_arg);
67 
68  // add point to octree
69  this->addPointsFromInputCloud ();
70 
71  // make sure cloud contains points
72  if (this->leafCount_>0) {
73 
74 
75  // color field analysis
76  cloudWithColor_ = false;
77  std::vector<sensor_msgs::PointField> fields;
78  int rgba_index = -1;
79  rgba_index = pcl::getFieldIndex (*this->input_, "rgb", fields);
80  if (rgba_index == -1)
81  {
82  rgba_index = pcl::getFieldIndex (*this->input_, "rgba", fields);
83  }
84  if (rgba_index >= 0)
85  {
86  pointColorOffset_ = static_cast<unsigned char> (fields[rgba_index].offset);
87  cloudWithColor_ = true;
88  }
89 
90  // apply encoding configuration
91  cloudWithColor_ &= doColorEncoding_;
92 
93 
94  // if octree depth changed, we enforce I-frame encoding
95  iFrame_ |= (recentTreeDepth != this->getTreeDepth ());// | !(iFrameCounter%10);
96 
97  // enable I-frame rate
98  if (iFrameCounter_++==iFrameRate_)
99  {
100  iFrameCounter_ =0;
101  iFrame_ = true;
102  }
103 
104  // increase frameID
105  frameID_++;
106 
107  // do octree encoding
108  if (!doVoxelGridEnDecoding_)
109  {
110  pointCountDataVector_.clear ();
111  pointCountDataVector_.reserve (cloud_arg->points.size ());
112  }
113 
114  // initialize color encoding
115  colorCoder_.initializeEncoding ();
116  colorCoder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
117  colorCoder_.setVoxelCount (static_cast<unsigned int> (this->leafCount_));
118 
119  // initialize point encoding
120  pointCoder_.initializeEncoding ();
121  pointCoder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
122 
123  // serialize octree
124  if (iFrame_)
125  // i-frame encoding - encode tree structure without referencing previous buffer
126  this->serializeTree (binaryTreeDataVector_, false);
127  else
128  // p-frame encoding - XOR encoded tree structure
129  this->serializeTree (binaryTreeDataVector_, true);
130 
131  // write frame header information to stream
132  this->writeFrameHeader (compressedTreeDataOut_arg);
133 
134  // apply entropy coding to the content of all data vectors and send data to output stream
135  this->entropyEncoding (compressedTreeDataOut_arg);
136 
137  // prepare for next frame
138  this->switchBuffers ();
139  iFrame_ = false;
140 
141  if (bShowStatistics)
142  {
143  float bytesPerXYZ = static_cast<float> (compressedPointDataLen_) / static_cast<float> (pointCount_);
144  float bytesPerColor = static_cast<float> (compressedColorDataLen_) / static_cast<float> (pointCount_);
145 
146  PCL_INFO ("*** POINTCLOUD ENCODING ***\n");
147  PCL_INFO ("Frame ID: %d\n", frameID_);
148  if (iFrame_)
149  PCL_INFO ("Encoding Frame: Intra frame\n");
150  else
151  PCL_INFO ("Encoding Frame: Prediction frame\n");
152  PCL_INFO ("Number of encoded points: %ld\n", pointCount_);
153  PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof(float)) * 100.0f);
154  PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ);
155  PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f);
156  PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor);
157  PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024);
158  PCL_INFO ("Size of compressed point cloud: %d kBytes\n", (compressedPointDataLen_ + compressedColorDataLen_) / (1024));
159  PCL_INFO ("Total bytes per point: %f\n", bytesPerXYZ + bytesPerColor);
160  PCL_INFO ("Total compression percentage: %f\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof(float)) * 100.0f);
161  PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytesPerXYZ + bytesPerColor));
162  }
163  } else {
164  if (bShowStatistics)
165  PCL_INFO ("Info: Dropping empty point cloud\n");
166  this->deleteTree();
167  iFrameCounter_ = 0;
168  iFrame_ = true;
169  }
170  }
171 
173  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
174  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::decodePointCloud (
175  std::istream& compressedTreeDataIn_arg,
176  PointCloudPtr &cloud_arg)
177  {
178 
179  // synchronize to frame header
180  syncToHeader(compressedTreeDataIn_arg);
181 
182  // initialize octree
183  this->switchBuffers ();
184  this->setOutputCloud (cloud_arg);
185 
186  // color field analysis
187  cloudWithColor_ = false;
188  std::vector<sensor_msgs::PointField> fields;
189  int rgba_index = -1;
190  rgba_index = pcl::getFieldIndex (*output_, "rgb", fields);
191  if (rgba_index == -1)
192  rgba_index = pcl::getFieldIndex (*output_, "rgba", fields);
193  if (rgba_index >= 0)
194  {
195  pointColorOffset_ = static_cast<unsigned char> (fields[rgba_index].offset);
196  cloudWithColor_ = true;
197  }
198 
199  // read header from input stream
200  this->readFrameHeader (compressedTreeDataIn_arg);
201 
202  // decode data vectors from stream
203  this->entropyDecoding (compressedTreeDataIn_arg);
204 
205  // initialize color and point encoding
206  colorCoder_.initializeDecoding ();
207  pointCoder_.initializeDecoding ();
208 
209  // initialize output cloud
210  output_->points.clear ();
211  output_->points.reserve (static_cast<std::size_t> (pointCount_));
212 
213  if (iFrame_)
214  // i-frame decoding - decode tree structure without referencing previous buffer
215  this->deserializeTree (binaryTreeDataVector_, false);
216  else
217  // p-frame decoding - decode XOR encoded tree structure
218  this->deserializeTree (binaryTreeDataVector_, true);
219 
220  // assign point cloud properties
221  output_->height = 1;
222  output_->width = static_cast<uint32_t> (cloud_arg->points.size ());
223  output_->is_dense = false;
224 
225  if (bShowStatistics)
226  {
227  float bytesPerXYZ = static_cast<float> (compressedPointDataLen_) / static_cast<float> (pointCount_);
228  float bytesPerColor = static_cast<float> (compressedColorDataLen_) / static_cast<float> (pointCount_);
229 
230  PCL_INFO ("*** POINTCLOUD DECODING ***\n");
231  PCL_INFO ("Frame ID: %d\n", frameID_);
232  if (iFrame_)
233  PCL_INFO ("Encoding Frame: Intra frame\n");
234  else
235  PCL_INFO ("Encoding Frame: Prediction frame\n");
236  PCL_INFO ("Number of encoded points: %ld\n", pointCount_);
237  PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof (float)) * 100.0f);
238  PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ);
239  PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f);
240  PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor);
241  PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f);
242  PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast<float> (compressedPointDataLen_ + compressedColorDataLen_) / 1024.0f);
243  PCL_INFO ("Total bytes per point: %d bytes\n", static_cast<int> (bytesPerXYZ + bytesPerColor));
244  PCL_INFO ("Total compression percentage: %f%%\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f);
245  PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytesPerXYZ + bytesPerColor));
246  }
247  }
248 
250  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
251  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyEncoding (std::ostream& compressedTreeDataOut_arg)
252  {
253  uint64_t binaryTreeDataVector_size;
254  uint64_t pointAvgColorDataVector_size;
255 
256  compressedPointDataLen_ = 0;
257  compressedColorDataLen_ = 0;
258 
259  // encode binary octree structure
260  binaryTreeDataVector_size = binaryTreeDataVector_.size ();
261  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size));
262  compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (binaryTreeDataVector_,
263  compressedTreeDataOut_arg);
264 
265  if (cloudWithColor_)
266  {
267  // encode averaged voxel color information
268  std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
269  pointAvgColorDataVector_size = pointAvgColorDataVector.size ();
270  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointAvgColorDataVector_size),
271  sizeof (pointAvgColorDataVector_size));
272  compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointAvgColorDataVector,
273  compressedTreeDataOut_arg);
274  }
275 
276  if (!doVoxelGridEnDecoding_)
277  {
278  uint64_t pointCountDataVector_size;
279  uint64_t pointDiffDataVector_size;
280  uint64_t pointDiffColorDataVector_size;
281 
282  // encode amount of points per voxel
283  pointCountDataVector_size = pointCountDataVector_.size ();
284  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
285  compressedPointDataLen_ += entropyCoder_.encodeIntVectorToStream (pointCountDataVector_,
286  compressedTreeDataOut_arg);
287 
288  // encode differential point information
289  std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
290  pointDiffDataVector_size = pointDiffDataVector.size ();
291  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size));
292  compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffDataVector,
293  compressedTreeDataOut_arg);
294  if (cloudWithColor_)
295  {
296  // encode differential color information
297  std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
298  pointDiffColorDataVector_size = pointDiffColorDataVector.size ();
299  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointDiffColorDataVector_size),
300  sizeof (pointDiffColorDataVector_size));
301  compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffColorDataVector,
302  compressedTreeDataOut_arg);
303  }
304  }
305  // flush output stream
306  compressedTreeDataOut_arg.flush ();
307  }
308 
310  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
311  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyDecoding (std::istream& compressedTreeDataIn_arg)
312  {
313  uint64_t binaryTreeDataVector_size;
314  uint64_t pointAvgColorDataVector_size;
315 
316  compressedPointDataLen_ = 0;
317  compressedColorDataLen_ = 0;
318 
319  // decode binary octree structure
320  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size));
321  binaryTreeDataVector_.resize (static_cast<std::size_t> (binaryTreeDataVector_size));
322  compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
323  binaryTreeDataVector_);
324 
325  if (dataWithColor_)
326  {
327  // decode averaged voxel color information
328  std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
329  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointAvgColorDataVector_size), sizeof (pointAvgColorDataVector_size));
330  pointAvgColorDataVector.resize (static_cast<std::size_t> (pointAvgColorDataVector_size));
331  compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
332  pointAvgColorDataVector);
333  }
334 
335  if (!doVoxelGridEnDecoding_)
336  {
337  uint64_t pointCountDataVector_size;
338  uint64_t pointDiffDataVector_size;
339  uint64_t pointDiffColorDataVector_size;
340 
341  // decode amount of points per voxel
342  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
343  pointCountDataVector_.resize (static_cast<std::size_t> (pointCountDataVector_size));
344  compressedPointDataLen_ += entropyCoder_.decodeStreamToIntVector (compressedTreeDataIn_arg, pointCountDataVector_);
345  pointCountDataVectorIterator_ = pointCountDataVector_.begin ();
346 
347  // decode differential point information
348  std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
349  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size));
350  pointDiffDataVector.resize (static_cast<std::size_t> (pointDiffDataVector_size));
351  compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
352  pointDiffDataVector);
353 
354  if (dataWithColor_)
355  {
356  // decode differential color information
357  std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
358  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointDiffColorDataVector_size), sizeof (pointDiffColorDataVector_size));
359  pointDiffColorDataVector.resize (static_cast<std::size_t> (pointDiffColorDataVector_size));
360  compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
361  pointDiffColorDataVector);
362  }
363  }
364  }
365 
367  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
368  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::writeFrameHeader (std::ostream& compressedTreeDataOut_arg)
369  {
370  // encode header identifier
371  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
372  // encode point cloud header id
373  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&frameID_), sizeof (frameID_));
374  // encode frame type (I/P-frame)
375  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&iFrame_), sizeof (iFrame_));
376  if (iFrame_)
377  {
378  double minX, minY, minZ, maxX, maxY, maxZ;
379  double octreeResolution;
380  unsigned char colorBitDepth;
381  double pointResolution;
382 
383  // get current configuration
384  octreeResolution = this->getResolution ();
385  colorBitDepth = colorCoder_.getBitDepth ();
386  pointResolution= pointCoder_.getPrecision ();
387  this->getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
388 
389  // encode amount of points
390  if (doVoxelGridEnDecoding_)
391  pointCount_ = this->leafCount_;
392  else
393  pointCount_ = this->objectCount_;
394 
395  // encode coding configuration
396  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_));
397  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&cloudWithColor_), sizeof (cloudWithColor_));
398  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointCount_), sizeof (pointCount_));
399  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&octreeResolution), sizeof (octreeResolution));
400  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&colorBitDepth), sizeof (colorBitDepth));
401  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointResolution), sizeof (pointResolution));
402 
403  // encode octree bounding box
404  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minX), sizeof (minX));
405  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minY), sizeof (minY));
406  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minZ), sizeof (minZ));
407  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxX), sizeof (maxX));
408  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxY), sizeof (maxY));
409  compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxZ), sizeof (maxZ));
410  }
411  }
412 
414  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
415  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::syncToHeader ( std::istream& compressedTreeDataIn_arg)
416  {
417  // sync to frame header
418  unsigned int headerIdPos = 0;
419  while (headerIdPos < strlen (frameHeaderIdentifier_))
420  {
421  char readChar;
422  compressedTreeDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
423  if (readChar != frameHeaderIdentifier_[headerIdPos++])
424  headerIdPos = (frameHeaderIdentifier_[0]==readChar)?1:0;
425  }
426  }
427 
429  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
430  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::readFrameHeader ( std::istream& compressedTreeDataIn_arg)
431  {
432  // read header
433  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&frameID_), sizeof (frameID_));
434  compressedTreeDataIn_arg.read (reinterpret_cast<char*>(&iFrame_), sizeof (iFrame_));
435  if (iFrame_)
436  {
437  double minX, minY, minZ, maxX, maxY, maxZ;
438  double octreeResolution;
439  unsigned char colorBitDepth;
440  double pointResolution;
441 
442  // read coder configuration
443  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_));
444  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&dataWithColor_), sizeof (dataWithColor_));
445  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointCount_), sizeof (pointCount_));
446  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&octreeResolution), sizeof (octreeResolution));
447  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&colorBitDepth), sizeof (colorBitDepth));
448  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointResolution), sizeof (pointResolution));
449 
450  // read octree bounding box
451  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minX), sizeof (minX));
452  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minY), sizeof (minY));
453  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minZ), sizeof (minZ));
454  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxX), sizeof (maxX));
455  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxY), sizeof (maxY));
456  compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxZ), sizeof (maxZ));
457 
458  // reset octree and assign new bounding box & resolution
459  this->deleteTree ();
460  this->setResolution (octreeResolution);
461  this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
462 
463  // configure color & point coding
464  colorCoder_.setBitDepth (colorBitDepth);
465  pointCoder_.setPrecision (static_cast<float> (pointResolution));
466  }
467  }
468 
470  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
471  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::serializeTreeCallback (
472  LeafNode &leaf_arg, const OctreeKey & key_arg)
473  {
474  // reference to point indices vector stored within octree leaf
475  const std::vector<int>& leafIdx = leaf_arg.getDataTVector ();
476 
477  if (!doVoxelGridEnDecoding_)
478  {
479  double lowerVoxelCorner[3];
480 
481  // encode amount of points within voxel
482  pointCountDataVector_.push_back (static_cast<int> (leafIdx.size ()));
483 
484  // calculate lower voxel corner based on octree key
485  lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->minX_;
486  lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->minY_;
487  lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->minZ_;
488 
489  // differentially encode points to lower voxel corner
490  pointCoder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
491 
492  if (cloudWithColor_)
493  // encode color of points
494  colorCoder_.encodePoints (leafIdx, pointColorOffset_, this->input_);
495  }
496  else
497  {
498  if (cloudWithColor_)
499  // encode average color of all points within voxel
500  colorCoder_.encodeAverageOfPoints (leafIdx, pointColorOffset_, this->input_);
501  }
502  }
503 
505  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
506  PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::deserializeTreeCallback (LeafNode&,
507  const OctreeKey& key_arg)
508  {
509  double lowerVoxelCorner[3];
510  std::size_t pointCount, i, cloudSize;
511  PointT newPoint;
512 
513  pointCount = 1;
514 
515  if (!doVoxelGridEnDecoding_)
516  {
517  // get current cloud size
518  cloudSize = output_->points.size ();
519 
520  // get amount of point to be decoded
521  pointCount = *pointCountDataVectorIterator_;
522  pointCountDataVectorIterator_++;
523 
524  // increase point cloud by amount of voxel points
525  for (i = 0; i < pointCount; i++)
526  output_->points.push_back (newPoint);
527 
528  // calculcate position of lower voxel corner
529  lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->minX_;
530  lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->minY_;
531  lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->minZ_;
532 
533  // decode differentially encoded points
534  pointCoder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount);
535  }
536  else
537  {
538  // calculate center of lower voxel corner
539  newPoint.x = static_cast<float> ((static_cast<double> (key_arg.x) + 0.5) * this->resolution_ + this->minX_);
540  newPoint.y = static_cast<float> ((static_cast<double> (key_arg.y) + 0.5) * this->resolution_ + this->minY_);
541  newPoint.z = static_cast<float> ((static_cast<double> (key_arg.z) + 0.5) * this->resolution_ + this->minZ_);
542 
543  // add point to point cloud
544  output_->points.push_back (newPoint);
545  }
546 
547  if (cloudWithColor_)
548  {
549  if (dataWithColor_)
550  // decode color information
551  colorCoder_.decodePoints (output_, output_->points.size () - pointCount,
552  output_->points.size (), pointColorOffset_);
553  else
554  // set default color information
555  colorCoder_.setDefaultColor (output_, output_->points.size () - pointCount,
556  output_->points.size (), pointColorOffset_);
557  }
558  }
559  }
560 }
561 
562 #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> > >;
563 
564 #endif
565 
Generated on Fri Mar 8 2013 12:50:50 for Point Cloud Library (PCL) by   doxygen 1.8.3.1