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