Point Cloud Library (PCL)  1.14.1-dev
octree_pointcloud_compression.hpp
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/common/io.h> // for getFieldIndex
42 #include <pcl/compression/entropy_range_coder.h>
43 
44 #include <iostream>
45 #include <vector>
46 #include <cstring>
47 
48 namespace pcl
49 {
50  namespace io
51  {
52  //////////////////////////////////////////////////////////////////////////////////////////////
53  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void OctreePointCloudCompression<
54  PointT, LeafT, BranchT, OctreeT>::encodePointCloud (
55  const PointCloudConstPtr &cloud_arg,
56  std::ostream& compressed_tree_data_out_arg)
57  {
58  auto recent_tree_depth =
59  static_cast<unsigned char> (this->getTreeDepth ());
60 
61  // initialize octree
62  this->setInputCloud (cloud_arg);
63 
64  // add point to octree
65  this->addPointsFromInputCloud ();
66 
67  // make sure cloud contains points
68  if (this->leaf_count_>0) {
69 
70 
71  // color field analysis
72  cloud_with_color_ = false;
73  std::vector<pcl::PCLPointField> fields;
74  int rgba_index = -1;
75  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
76  if (rgba_index == -1)
77  {
78  rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
79  }
80  if (rgba_index >= 0)
81  {
82  point_color_offset_ = static_cast<unsigned char> (fields[rgba_index].offset);
83  cloud_with_color_ = true;
84  }
85 
86  // apply encoding configuration
87  cloud_with_color_ &= do_color_encoding_;
88 
89 
90  // if octree depth changed, we enforce I-frame encoding
91  i_frame_ |= (recent_tree_depth != this->getTreeDepth ());// | !(iFrameCounter%10);
92 
93  // enable I-frame rate
94  if (i_frame_counter_++==i_frame_rate_)
95  {
96  i_frame_counter_ =0;
97  i_frame_ = true;
98  }
99 
100  // increase frameID
101  frame_ID_++;
102 
103  // do octree encoding
104  if (!do_voxel_grid_enDecoding_)
105  {
106  point_count_data_vector_.clear ();
107  point_count_data_vector_.reserve (cloud_arg->size ());
108  }
109 
110  // initialize color encoding
111  color_coder_.initializeEncoding ();
112  color_coder_.setPointCount (static_cast<unsigned int> (cloud_arg->size ()));
113  color_coder_.setVoxelCount (static_cast<unsigned int> (this->leaf_count_));
114 
115  // initialize point encoding
116  point_coder_.initializeEncoding ();
117  point_coder_.setPointCount (static_cast<unsigned int> (cloud_arg->size ()));
118 
119  // serialize octree
120  if (i_frame_)
121  // i-frame encoding - encode tree structure without referencing previous buffer
122  this->serializeTree (binary_tree_data_vector_, false);
123  else
124  // p-frame encoding - XOR encoded tree structure
125  this->serializeTree (binary_tree_data_vector_, true);
126 
127 
128  // write frame header information to stream
129  this->writeFrameHeader (compressed_tree_data_out_arg);
130 
131  // apply entropy coding to the content of all data vectors and send data to output stream
132  this->entropyEncoding (compressed_tree_data_out_arg);
133 
134  // prepare for next frame
135  this->switchBuffers ();
136 
137  // reset object count
138  object_count_ = 0;
139 
140  if (b_show_statistics_)
141  {
142  float bytes_per_XYZ = static_cast<float> (compressed_point_data_len_) / static_cast<float> (point_count_);
143  float bytes_per_color = static_cast<float> (compressed_color_data_len_) / static_cast<float> (point_count_);
144 
145  PCL_INFO ("*** POINTCLOUD ENCODING ***\n");
146  PCL_INFO ("Frame ID: %d\n", frame_ID_);
147  if (i_frame_)
148  PCL_INFO ("Encoding Frame: Intra frame\n");
149  else
150  PCL_INFO ("Encoding Frame: Prediction frame\n");
151  PCL_INFO ("Number of encoded points: %ld\n", point_count_);
152  PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof (float)) * 100.0f);
153  PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ);
154  PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f);
155  PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color);
156  PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f);
157  PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast<float> (compressed_point_data_len_ + compressed_color_data_len_) / 1024.0f);
158  PCL_INFO ("Total bytes per point: %f bytes\n", bytes_per_XYZ + bytes_per_color);
159  PCL_INFO ("Total compression percentage: %f%%\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f);
160  PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytes_per_XYZ + bytes_per_color));
161  }
162 
163  i_frame_ = false;
164  } else {
165  if (b_show_statistics_)
166  PCL_INFO ("Info: Dropping empty point cloud\n");
167  this->deleteTree();
168  i_frame_counter_ = 0;
169  i_frame_ = true;
170  }
171  }
172 
173  //////////////////////////////////////////////////////////////////////////////////////////////
174  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
176  std::istream& compressed_tree_data_in_arg,
177  PointCloudPtr &cloud_arg)
178  {
179 
180  // synchronize to frame header
181  syncToHeader(compressed_tree_data_in_arg);
182 
183  // initialize octree
184  this->switchBuffers ();
185  this->setOutputCloud (cloud_arg);
186 
187  // color field analysis
188  cloud_with_color_ = false;
189  std::vector<pcl::PCLPointField> fields;
190  int rgba_index = -1;
191  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
192  if (rgba_index == -1)
193  rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
194  if (rgba_index >= 0)
195  {
196  point_color_offset_ = static_cast<unsigned char> (fields[rgba_index].offset);
197  cloud_with_color_ = true;
198  }
199 
200  // read header from input stream
201  this->readFrameHeader (compressed_tree_data_in_arg);
202 
203  // decode data vectors from stream
204  this->entropyDecoding (compressed_tree_data_in_arg);
205 
206  // initialize color and point encoding
207  color_coder_.initializeDecoding ();
208  point_coder_.initializeDecoding ();
209 
210  // initialize output cloud
211  output_->points.clear ();
212  output_->points.reserve (static_cast<std::size_t> (point_count_));
213 
214  if (i_frame_)
215  // i-frame decoding - decode tree structure without referencing previous buffer
216  this->deserializeTree (binary_tree_data_vector_, false);
217  else
218  // p-frame decoding - decode XOR encoded tree structure
219  this->deserializeTree (binary_tree_data_vector_, true);
220 
221  // assign point cloud properties
222  output_->height = 1;
223  output_->width = cloud_arg->size ();
224  output_->is_dense = false;
225 
226  if (b_show_statistics_)
227  {
228  float bytes_per_XYZ = static_cast<float> (compressed_point_data_len_) / static_cast<float> (point_count_);
229  float bytes_per_color = static_cast<float> (compressed_color_data_len_) / static_cast<float> (point_count_);
230 
231  PCL_INFO ("*** POINTCLOUD DECODING ***\n");
232  PCL_INFO ("Frame ID: %d\n", frame_ID_);
233  if (i_frame_)
234  PCL_INFO ("Decoding Frame: Intra frame\n");
235  else
236  PCL_INFO ("Decoding Frame: Prediction frame\n");
237  PCL_INFO ("Number of decoded points: %ld\n", point_count_);
238  PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof (float)) * 100.0f);
239  PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ);
240  PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f);
241  PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color);
242  PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f);
243  PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast<float> (compressed_point_data_len_ + compressed_color_data_len_) / 1024.0f);
244  PCL_INFO ("Total bytes per point: %f bytes\n", bytes_per_XYZ + bytes_per_color);
245  PCL_INFO ("Total compression percentage: %f%%\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f);
246  PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytes_per_XYZ + bytes_per_color));
247  }
248  }
249 
250  //////////////////////////////////////////////////////////////////////////////////////////////
251  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
253  {
254  std::uint64_t binary_tree_data_vector_size;
255  std::uint64_t point_avg_color_data_vector_size;
256 
257  compressed_point_data_len_ = 0;
258  compressed_color_data_len_ = 0;
259 
260  // encode binary octree structure
261  binary_tree_data_vector_size = binary_tree_data_vector_.size ();
262  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size));
263  compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (binary_tree_data_vector_,
264  compressed_tree_data_out_arg);
265 
266  if (cloud_with_color_)
267  {
268  // encode averaged voxel color information
269  std::vector<char>& pointAvgColorDataVector = color_coder_.getAverageDataVector ();
270  point_avg_color_data_vector_size = pointAvgColorDataVector.size ();
271  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_avg_color_data_vector_size),
272  sizeof (point_avg_color_data_vector_size));
273  compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (pointAvgColorDataVector,
274  compressed_tree_data_out_arg);
275  }
276 
277  if (!do_voxel_grid_enDecoding_)
278  {
279  std::uint64_t pointCountDataVector_size;
280  std::uint64_t point_diff_data_vector_size;
281  std::uint64_t point_diff_color_data_vector_size;
282 
283  // encode amount of points per voxel
284  pointCountDataVector_size = point_count_data_vector_.size ();
285  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
286  compressed_point_data_len_ += entropy_coder_.encodeIntVectorToStream (point_count_data_vector_,
287  compressed_tree_data_out_arg);
288 
289  // encode differential point information
290  std::vector<char>& point_diff_data_vector = point_coder_.getDifferentialDataVector ();
291  point_diff_data_vector_size = point_diff_data_vector.size ();
292  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size));
293  compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_data_vector,
294  compressed_tree_data_out_arg);
295  if (cloud_with_color_)
296  {
297  // encode differential color information
298  std::vector<char>& point_diff_color_data_vector = color_coder_.getDifferentialDataVector ();
299  point_diff_color_data_vector_size = point_diff_color_data_vector.size ();
300  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_diff_color_data_vector_size),
301  sizeof (point_diff_color_data_vector_size));
302  compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_color_data_vector,
303  compressed_tree_data_out_arg);
304  }
305  }
306  // flush output stream
307  compressed_tree_data_out_arg.flush ();
308  }
309 
310  //////////////////////////////////////////////////////////////////////////////////////////////
311  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
313  {
314  std::uint64_t binary_tree_data_vector_size;
315  std::uint64_t point_avg_color_data_vector_size;
316 
317  compressed_point_data_len_ = 0;
318  compressed_color_data_len_ = 0;
319 
320  // decode binary octree structure
321  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size));
322  binary_tree_data_vector_.resize (static_cast<std::size_t> (binary_tree_data_vector_size));
323  compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg,
324  binary_tree_data_vector_);
325 
326  if (data_with_color_)
327  {
328  // decode averaged voxel color information
329  std::vector<char>& point_avg_color_data_vector = color_coder_.getAverageDataVector ();
330  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_avg_color_data_vector_size), sizeof (point_avg_color_data_vector_size));
331  point_avg_color_data_vector.resize (static_cast<std::size_t> (point_avg_color_data_vector_size));
332  compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg,
333  point_avg_color_data_vector);
334  }
335 
336  if (!do_voxel_grid_enDecoding_)
337  {
338  std::uint64_t point_count_data_vector_size;
339  std::uint64_t point_diff_data_vector_size;
340  std::uint64_t point_diff_color_data_vector_size;
341 
342  // decode amount of points per voxel
343  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_count_data_vector_size), sizeof (point_count_data_vector_size));
344  point_count_data_vector_.resize (static_cast<std::size_t> (point_count_data_vector_size));
345  compressed_point_data_len_ += entropy_coder_.decodeStreamToIntVector (compressed_tree_data_in_arg, point_count_data_vector_);
346  point_count_data_vector_iterator_ = point_count_data_vector_.begin ();
347 
348  // decode differential point information
349  std::vector<char>& pointDiffDataVector = point_coder_.getDifferentialDataVector ();
350  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size));
351  pointDiffDataVector.resize (static_cast<std::size_t> (point_diff_data_vector_size));
352  compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg,
353  pointDiffDataVector);
354 
355  if (data_with_color_)
356  {
357  // decode differential color information
358  std::vector<char>& pointDiffColorDataVector = color_coder_.getDifferentialDataVector ();
359  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_diff_color_data_vector_size), sizeof (point_diff_color_data_vector_size));
360  pointDiffColorDataVector.resize (static_cast<std::size_t> (point_diff_color_data_vector_size));
361  compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg,
362  pointDiffColorDataVector);
363  }
364  }
365  }
366 
367  //////////////////////////////////////////////////////////////////////////////////////////////
368  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
370  {
371  // encode header identifier
372  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (frame_header_identifier_), strlen (frame_header_identifier_));
373  // encode point cloud header id
374  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&frame_ID_), sizeof (frame_ID_));
375  // encode frame type (I/P-frame)
376  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&i_frame_), sizeof (i_frame_));
377  if (i_frame_)
378  {
379  double min_x, min_y, min_z, max_x, max_y, max_z;
380  double octree_resolution;
381  unsigned char color_bit_depth;
382  double point_resolution;
383 
384  // get current configuration
385  octree_resolution = this->getResolution ();
386  color_bit_depth = color_coder_.getBitDepth ();
387  point_resolution= point_coder_.getPrecision ();
388  this->getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
389 
390  // encode amount of points
391  if (do_voxel_grid_enDecoding_)
392  point_count_ = this->leaf_count_;
393  else
394  point_count_ = this->object_count_;
395 
396  // encode coding configuration
397  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_));
398  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&cloud_with_color_), sizeof (cloud_with_color_));
399  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_count_), sizeof (point_count_));
400  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&octree_resolution), sizeof (octree_resolution));
401  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&color_bit_depth), sizeof (color_bit_depth));
402  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_resolution), sizeof (point_resolution));
403 
404  // encode octree bounding box
405  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&min_x), sizeof (min_x));
406  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&min_y), sizeof (min_y));
407  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&min_z), sizeof (min_z));
408  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&max_x), sizeof (max_x));
409  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&max_y), sizeof (max_y));
410  compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&max_z), sizeof (max_z));
411  }
412  }
413 
414  //////////////////////////////////////////////////////////////////////////////////////////////
415  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
417  {
418  // sync to frame header
419  unsigned int header_id_pos = 0;
420  while (header_id_pos < strlen (frame_header_identifier_))
421  {
422  char readChar;
423  compressed_tree_data_in_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
424  if (readChar != frame_header_identifier_[header_id_pos++])
425  header_id_pos = (frame_header_identifier_[0]==readChar)?1:0;
426  }
427  }
428 
429  //////////////////////////////////////////////////////////////////////////////////////////////
430  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
432  {
433  // read header
434  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&frame_ID_), sizeof (frame_ID_));
435  compressed_tree_data_in_arg.read (reinterpret_cast<char*>(&i_frame_), sizeof (i_frame_));
436  if (i_frame_)
437  {
438  double min_x, min_y, min_z, max_x, max_y, max_z;
439  double octree_resolution;
440  unsigned char color_bit_depth;
441  double point_resolution;
442 
443  // read coder configuration
444  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_));
445  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&data_with_color_), sizeof (data_with_color_));
446  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_count_), sizeof (point_count_));
447  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&octree_resolution), sizeof (octree_resolution));
448  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&color_bit_depth), sizeof (color_bit_depth));
449  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_resolution), sizeof (point_resolution));
450 
451  // read octree bounding box
452  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&min_x), sizeof (min_x));
453  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&min_y), sizeof (min_y));
454  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&min_z), sizeof (min_z));
455  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&max_x), sizeof (max_x));
456  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&max_y), sizeof (max_y));
457  compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&max_z), sizeof (max_z));
458 
459  // reset octree and assign new bounding box & resolution
460  this->deleteTree ();
461  this->setResolution (octree_resolution);
462  this->defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
463 
464  // configure color & point coding
465  color_coder_.setBitDepth (color_bit_depth);
466  point_coder_.setPrecision (static_cast<float> (point_resolution));
467  }
468  }
469 
470  //////////////////////////////////////////////////////////////////////////////////////////////
471  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
473  LeafT &leaf_arg, const OctreeKey & key_arg)
474  {
475  // reference to point indices vector stored within octree leaf
476  const auto& leafIdx = leaf_arg.getPointIndicesVector();
477 
478  if (!do_voxel_grid_enDecoding_)
479  {
480  double lowerVoxelCorner[3];
481 
482  // encode amount of points within voxel
483  point_count_data_vector_.push_back (static_cast<int> (leafIdx.size ()));
484 
485  // calculate lower voxel corner based on octree key
486  lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->min_x_;
487  lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->min_y_;
488  lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->min_z_;
489 
490  // differentially encode points to lower voxel corner
491  point_coder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
492 
493  if (cloud_with_color_)
494  // encode color of points
495  color_coder_.encodePoints (leafIdx, point_color_offset_, this->input_);
496  }
497  else
498  {
499  if (cloud_with_color_)
500  // encode average color of all points within voxel
501  color_coder_.encodeAverageOfPoints (leafIdx, point_color_offset_, this->input_);
502  }
503  }
504 
505  //////////////////////////////////////////////////////////////////////////////////////////////
506  template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
508  const OctreeKey& key_arg)
509  {
510  PointT newPoint;
511 
512  std::size_t pointCount = 1;
513 
514  if (!do_voxel_grid_enDecoding_)
515  {
516  // get current cloud size
517  const auto cloudSize = output_->size ();
518 
519  // get amount of point to be decoded
520  pointCount = *point_count_data_vector_iterator_;
521  point_count_data_vector_iterator_++;
522 
523  // increase point cloud by amount of voxel points
524  for (std::size_t i = 0; i < pointCount; i++)
525  output_->points.push_back (newPoint);
526 
527  // calculate position of lower voxel corner
528  double lowerVoxelCorner[3];
529  lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->min_x_;
530  lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->min_y_;
531  lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->min_z_;
532 
533  // decode differentially encoded points
534  point_coder_.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->min_x_);
540  newPoint.y = static_cast<float> ((static_cast<double> (key_arg.y) + 0.5) * this->resolution_ + this->min_y_);
541  newPoint.z = static_cast<float> ((static_cast<double> (key_arg.z) + 0.5) * this->resolution_ + this->min_z_);
542 
543  // add point to point cloud
544  output_->points.push_back (newPoint);
545  }
546 
547  if (cloud_with_color_)
548  {
549  if (data_with_color_)
550  // decode color information
551  color_coder_.decodePoints (output_, output_->size () - pointCount,
552  output_->size (), point_color_offset_);
553  else
554  // set default color information
555  color_coder_.setDefaultColor (output_, output_->size () - pointCount,
556  output_->size (), point_color_offset_);
557  }
558  }
559  }
560 }
561 
562 #endif
563 
void entropyEncoding(std::ostream &compressed_tree_data_out_arg)
Apply entropy encoding to encoded information and output to binary stream.
void syncToHeader(std::istream &compressed_tree_data_in_arg)
Synchronize to frame header.
void readFrameHeader(std::istream &compressed_tree_data_in_arg)
Read frame information to output stream.
void writeFrameHeader(std::ostream &compressed_tree_data_out_arg)
Write frame information to output stream.
typename OctreePointCloud< PointT, LeafT, BranchT, OctreeT >::PointCloudConstPtr PointCloudConstPtr
void entropyDecoding(std::istream &compressed_tree_data_in_arg)
Entropy decoding of input binary stream and output to information vectors.
void decodePointCloud(std::istream &compressed_tree_data_in_arg, PointCloudPtr &cloud_arg)
Decode point cloud from input stream.
void deserializeTreeCallback(LeafT &, const OctreeKey &key_arg) override
Decode leaf nodes information during deserialization.
void serializeTreeCallback(LeafT &leaf_arg, const OctreeKey &key_arg) override
Encode leaf node information during serialization.
typename OctreePointCloud< PointT, LeafT, BranchT, OctreeT >::PointCloudPtr PointCloudPtr
Octree double buffer class
A point structure representing Euclidean xyz coordinates, and the RGB color.