38 #ifndef OCTREE_COMPRESSION_HPP
39 #define OCTREE_COMPRESSION_HPP
41 #include <pcl/octree/octree_pointcloud.h>
42 #include <pcl/compression/entropy_range_coder.h>
58 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void OctreePointCloudCompression<
59 PointT, LeafT, BranchT, OctreeT>::encodePointCloud (
61 std::ostream& compressed_tree_data_out_arg)
63 unsigned char recent_tree_depth =
64 static_cast<unsigned char> (this->getTreeDepth ());
67 this->setInputCloud (cloud_arg);
70 this->addPointsFromInputCloud ();
73 if (this->leaf_count_>0) {
77 cloud_with_color_ =
false;
78 std::vector<pcl::PCLPointField> fields;
87 point_color_offset_ =
static_cast<unsigned char> (fields[rgba_index].offset);
88 cloud_with_color_ =
true;
92 cloud_with_color_ &= do_color_encoding_;
96 i_frame_ |= (recent_tree_depth != this->getTreeDepth ());
99 if (i_frame_counter_++==i_frame_rate_)
109 if (!do_voxel_grid_enDecoding_)
111 point_count_data_vector_.clear ();
112 point_count_data_vector_.reserve (cloud_arg->points.size ());
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_));
121 point_coder_.initializeEncoding ();
122 point_coder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
127 this->serializeTree (binary_tree_data_vector_,
false);
130 this->serializeTree (binary_tree_data_vector_,
true);
134 this->writeFrameHeader (compressed_tree_data_out_arg);
137 this->entropyEncoding (compressed_tree_data_out_arg);
140 this->switchBuffers ();
146 if (b_show_statistics_)
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_);
151 PCL_INFO (
"*** POINTCLOUD ENCODING ***\n");
152 PCL_INFO (
"Frame ID: %d\n", frame_ID_);
154 PCL_INFO (
"Encoding Frame: Intra frame\n");
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));
169 if (b_show_statistics_)
170 PCL_INFO (
"Info: Dropping empty point cloud\n");
172 i_frame_counter_ = 0;
178 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
180 std::istream& compressed_tree_data_in_arg,
185 syncToHeader(compressed_tree_data_in_arg);
188 this->switchBuffers ();
189 this->setOutputCloud (cloud_arg);
192 cloud_with_color_ =
false;
193 std::vector<pcl::PCLPointField> fields;
196 if (rgba_index == -1)
200 point_color_offset_ =
static_cast<unsigned char> (fields[rgba_index].offset);
201 cloud_with_color_ =
true;
205 this->readFrameHeader (compressed_tree_data_in_arg);
208 this->entropyDecoding (compressed_tree_data_in_arg);
211 color_coder_.initializeDecoding ();
212 point_coder_.initializeDecoding ();
215 output_->points.clear ();
216 output_->points.reserve (static_cast<std::size_t> (point_count_));
220 this->deserializeTree (binary_tree_data_vector_,
false);
223 this->deserializeTree (binary_tree_data_vector_,
true);
227 output_->width =
static_cast<uint32_t
> (cloud_arg->points.size ());
228 output_->is_dense =
false;
230 if (b_show_statistics_)
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_);
235 PCL_INFO (
"*** POINTCLOUD DECODING ***\n");
236 PCL_INFO (
"Frame ID: %d\n", frame_ID_);
238 PCL_INFO (
"Encoding Frame: Intra frame\n");
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));
255 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
258 uint64_t binary_tree_data_vector_size;
259 uint64_t point_avg_color_data_vector_size;
261 compressed_point_data_len_ = 0;
262 compressed_color_data_len_ = 0;
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);
270 if (cloud_with_color_)
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);
281 if (!do_voxel_grid_enDecoding_)
283 uint64_t pointCountDataVector_size;
284 uint64_t point_diff_data_vector_size;
285 uint64_t point_diff_color_data_vector_size;
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);
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_)
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);
311 compressed_tree_data_out_arg.flush ();
315 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
318 uint64_t binary_tree_data_vector_size;
319 uint64_t point_avg_color_data_vector_size;
321 compressed_point_data_len_ = 0;
322 compressed_color_data_len_ = 0;
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_);
330 if (data_with_color_)
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);
340 if (!do_voxel_grid_enDecoding_)
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;
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 ();
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);
359 if (data_with_color_)
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);
372 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
376 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (frame_header_identifier_), strlen (frame_header_identifier_));
378 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&frame_ID_),
sizeof (frame_ID_));
380 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&i_frame_),
sizeof (i_frame_));
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;
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);
395 if (do_voxel_grid_enDecoding_)
396 point_count_ = this->leaf_count_;
398 point_count_ = this->object_count_;
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));
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));
419 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
423 unsigned int header_id_pos = 0;
424 while (header_id_pos < strlen (frame_header_identifier_))
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;
434 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
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_));
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;
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));
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));
465 this->setResolution (octree_resolution);
466 this->defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
469 color_coder_.setBitDepth (color_bit_depth);
470 point_coder_.setPrecision (static_cast<float> (point_resolution));
475 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
477 LeafT &leaf_arg,
const OctreeKey & key_arg)
480 const std::vector<int>& leafIdx = leaf_arg.getPointIndicesVector();
482 if (!do_voxel_grid_enDecoding_)
484 double lowerVoxelCorner[3];
487 point_count_data_vector_.push_back (static_cast<int> (leafIdx.size ()));
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_;
495 point_coder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
497 if (cloud_with_color_)
499 color_coder_.encodePoints (leafIdx, point_color_offset_, this->input_);
503 if (cloud_with_color_)
505 color_coder_.encodeAverageOfPoints (leafIdx, point_color_offset_, this->input_);
510 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
514 double lowerVoxelCorner[3];
515 std::size_t pointCount, i, cloudSize;
520 if (!do_voxel_grid_enDecoding_)
523 cloudSize = output_->points.size ();
526 pointCount = *point_count_data_vector_iterator_;
527 point_count_data_vector_iterator_++;
530 for (i = 0; i < pointCount; i++)
531 output_->points.push_back (newPoint);
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_;
539 point_coder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount);
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_);
549 output_->points.push_back (newPoint);
552 if (cloud_with_color_)
554 if (data_with_color_)
556 color_coder_.decodePoints (output_, output_->points.size () - pointCount,
557 output_->points.size (), point_color_offset_);
560 color_coder_.setDefaultColor (output_, output_->points.size () - pointCount,
561 output_->points.size (), point_color_offset_);
Octree pointcloud compression class
OctreePointCloud< PointT, LeafT, BranchT, OctreeT >::PointCloudConstPtr PointCloudConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
OctreePointCloud< PointT, LeafT, BranchT, OctreeT >::PointCloudPtr PointCloudPtr