Point Cloud Library (PCL) 1.12.1
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
48namespace 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 unsigned char 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 key class
Definition: octree_key.h:52
A point structure representing Euclidean xyz coordinates, and the RGB color.