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