38 #ifndef OCTREE_COMPRESSION_HPP
39 #define OCTREE_COMPRESSION_HPP
41 #include <pcl/compression/entropy_range_coder.h>
55 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void OctreePointCloudCompression<
56 PointT, LeafT, BranchT, OctreeT>::encodePointCloud (
58 std::ostream& compressed_tree_data_out_arg)
60 unsigned char recent_tree_depth =
61 static_cast<unsigned char> (this->getTreeDepth ());
64 this->setInputCloud (cloud_arg);
67 this->addPointsFromInputCloud ();
70 if (this->leaf_count_>0) {
74 cloud_with_color_ =
false;
75 std::vector<pcl::PCLPointField> fields;
77 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
80 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
84 point_color_offset_ = static_cast<unsigned char> (fields[rgba_index].offset);
85 cloud_with_color_ =
true;
89 cloud_with_color_ &= do_color_encoding_;
93 i_frame_ |= (recent_tree_depth != this->getTreeDepth ());
96 if (i_frame_counter_++==i_frame_rate_)
106 if (!do_voxel_grid_enDecoding_)
108 point_count_data_vector_.clear ();
109 point_count_data_vector_.reserve (cloud_arg->points.size ());
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_));
118 point_coder_.initializeEncoding ();
119 point_coder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
124 this->serializeTree (binary_tree_data_vector_,
false);
127 this->serializeTree (binary_tree_data_vector_,
true);
131 this->writeFrameHeader (compressed_tree_data_out_arg);
134 this->entropyEncoding (compressed_tree_data_out_arg);
137 this->switchBuffers ();
142 if (b_show_statistics_)
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_);
147 PCL_INFO (
"*** POINTCLOUD ENCODING ***\n");
148 PCL_INFO (
"Frame ID: %d\n", frame_ID_);
150 PCL_INFO (
"Encoding Frame: Intra frame\n");
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));
167 if (b_show_statistics_)
168 PCL_INFO (
"Info: Dropping empty point cloud\n");
170 i_frame_counter_ = 0;
176 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
178 std::istream& compressed_tree_data_in_arg,
183 syncToHeader(compressed_tree_data_in_arg);
186 this->switchBuffers ();
187 this->setOutputCloud (cloud_arg);
190 cloud_with_color_ =
false;
191 std::vector<pcl::PCLPointField> fields;
193 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
194 if (rgba_index == -1)
195 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
198 point_color_offset_ = static_cast<unsigned char> (fields[rgba_index].offset);
199 cloud_with_color_ =
true;
203 this->readFrameHeader (compressed_tree_data_in_arg);
206 this->entropyDecoding (compressed_tree_data_in_arg);
209 color_coder_.initializeDecoding ();
210 point_coder_.initializeDecoding ();
213 output_->points.clear ();
214 output_->points.reserve (static_cast<std::size_t> (point_count_));
218 this->deserializeTree (binary_tree_data_vector_,
false);
221 this->deserializeTree (binary_tree_data_vector_,
true);
225 output_->width = static_cast<std::uint32_t> (cloud_arg->points.size ());
226 output_->is_dense =
false;
228 if (b_show_statistics_)
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_);
233 PCL_INFO (
"*** POINTCLOUD DECODING ***\n");
234 PCL_INFO (
"Frame ID: %d\n", frame_ID_);
236 PCL_INFO (
"Decoding Frame: Intra frame\n");
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));
253 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
259 compressed_point_data_len_ = 0;
260 compressed_color_data_len_ = 0;
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);
268 if (cloud_with_color_)
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);
279 if (!do_voxel_grid_enDecoding_)
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);
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_)
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);
309 compressed_tree_data_out_arg.flush ();
313 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
319 compressed_point_data_len_ = 0;
320 compressed_color_data_len_ = 0;
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_);
328 if (data_with_color_)
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);
338 if (!do_voxel_grid_enDecoding_)
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 ();
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);
357 if (data_with_color_)
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);
370 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
374 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (frame_header_identifier_), strlen (frame_header_identifier_));
376 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&frame_ID_),
sizeof (frame_ID_));
378 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&i_frame_),
sizeof (i_frame_));
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;
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);
393 if (do_voxel_grid_enDecoding_)
394 point_count_ = this->leaf_count_;
396 point_count_ = this->object_count_;
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));
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));
417 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
421 unsigned int header_id_pos = 0;
422 while (header_id_pos < strlen (frame_header_identifier_))
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;
432 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
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_));
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;
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));
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));
463 this->setResolution (octree_resolution);
464 this->defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
467 color_coder_.setBitDepth (color_bit_depth);
468 point_coder_.setPrecision (static_cast<float> (point_resolution));
473 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
475 LeafT &leaf_arg,
const OctreeKey & key_arg)
478 const std::vector<int>& leafIdx = leaf_arg.getPointIndicesVector();
480 if (!do_voxel_grid_enDecoding_)
482 double lowerVoxelCorner[3];
485 point_count_data_vector_.push_back (static_cast<int> (leafIdx.size ()));
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_;
493 point_coder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
495 if (cloud_with_color_)
497 color_coder_.encodePoints (leafIdx, point_color_offset_, this->input_);
501 if (cloud_with_color_)
503 color_coder_.encodeAverageOfPoints (leafIdx, point_color_offset_, this->input_);
508 template<
typename Po
intT,
typename LeafT,
typename BranchT,
typename OctreeT>
void
510 const OctreeKey& key_arg)
512 double lowerVoxelCorner[3];
515 std::size_t pointCount = 1;
517 if (!do_voxel_grid_enDecoding_)
520 std::size_t cloudSize = output_->points.size ();
523 pointCount = *point_count_data_vector_iterator_;
524 point_count_data_vector_iterator_++;
527 for (std::size_t i = 0; i < pointCount; i++)
528 output_->points.push_back (newPoint);
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_;
536 point_coder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount);
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_);
546 output_->points.push_back (newPoint);
549 if (cloud_with_color_)
551 if (data_with_color_)
553 color_coder_.decodePoints (output_, output_->points.size () - pointCount,
554 output_->points.size (), point_color_offset_);
557 color_coder_.setDefaultColor (output_, output_->points.size () - pointCount,
558 output_->points.size (), point_color_offset_);