Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef OCTREE_COMPRESSION_HPP 00039 #define OCTREE_COMPRESSION_HPP 00040 00041 #include <pcl/octree/octree_pointcloud.h> 00042 #include <pcl/compression/entropy_range_coder.h> 00043 00044 #include <iterator> 00045 #include <iostream> 00046 #include <vector> 00047 #include <string.h> 00048 #include <iostream> 00049 #include <stdio.h> 00050 00051 using namespace pcl::octree; 00052 00053 namespace pcl 00054 { 00055 namespace io 00056 { 00057 ////////////////////////////////////////////////////////////////////////////////////////////// 00058 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void OctreePointCloudCompression< 00059 PointT, LeafT, BranchT, OctreeT>::encodePointCloud ( 00060 const PointCloudConstPtr &cloud_arg, 00061 std::ostream& compressed_tree_data_out_arg) 00062 { 00063 unsigned char recent_tree_depth = 00064 static_cast<unsigned char> (this->getTreeDepth ()); 00065 00066 // initialize octree 00067 this->setInputCloud (cloud_arg); 00068 00069 // add point to octree 00070 this->addPointsFromInputCloud (); 00071 00072 // make sure cloud contains points 00073 if (this->leaf_count_>0) { 00074 00075 00076 // color field analysis 00077 cloud_with_color_ = false; 00078 std::vector<pcl::PCLPointField> fields; 00079 int rgba_index = -1; 00080 rgba_index = pcl::getFieldIndex (*this->input_, "rgb", fields); 00081 if (rgba_index == -1) 00082 { 00083 rgba_index = pcl::getFieldIndex (*this->input_, "rgba", fields); 00084 } 00085 if (rgba_index >= 0) 00086 { 00087 point_color_offset_ = static_cast<unsigned char> (fields[rgba_index].offset); 00088 cloud_with_color_ = true; 00089 } 00090 00091 // apply encoding configuration 00092 cloud_with_color_ &= do_color_encoding_; 00093 00094 00095 // if octree depth changed, we enforce I-frame encoding 00096 i_frame_ |= (recent_tree_depth != this->getTreeDepth ());// | !(iFrameCounter%10); 00097 00098 // enable I-frame rate 00099 if (i_frame_counter_++==i_frame_rate_) 00100 { 00101 i_frame_counter_ =0; 00102 i_frame_ = true; 00103 } 00104 00105 // increase frameID 00106 frame_ID_++; 00107 00108 // do octree encoding 00109 if (!do_voxel_grid_enDecoding_) 00110 { 00111 point_count_data_vector_.clear (); 00112 point_count_data_vector_.reserve (cloud_arg->points.size ()); 00113 } 00114 00115 // initialize color encoding 00116 color_coder_.initializeEncoding (); 00117 color_coder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ())); 00118 color_coder_.setVoxelCount (static_cast<unsigned int> (this->leaf_count_)); 00119 00120 // initialize point encoding 00121 point_coder_.initializeEncoding (); 00122 point_coder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ())); 00123 00124 // serialize octree 00125 if (i_frame_) 00126 // i-frame encoding - encode tree structure without referencing previous buffer 00127 this->serializeTree (binary_tree_data_vector_, false); 00128 else 00129 // p-frame encoding - XOR encoded tree structure 00130 this->serializeTree (binary_tree_data_vector_, true); 00131 00132 00133 // write frame header information to stream 00134 this->writeFrameHeader (compressed_tree_data_out_arg); 00135 00136 // apply entropy coding to the content of all data vectors and send data to output stream 00137 this->entropyEncoding (compressed_tree_data_out_arg); 00138 00139 // prepare for next frame 00140 this->switchBuffers (); 00141 i_frame_ = false; 00142 00143 // reset object count 00144 object_count_ = 0; 00145 00146 if (b_show_statistics_) 00147 { 00148 float bytes_per_XYZ = static_cast<float> (compressed_point_data_len_) / static_cast<float> (point_count_); 00149 float bytes_per_color = static_cast<float> (compressed_color_data_len_) / static_cast<float> (point_count_); 00150 00151 PCL_INFO ("*** POINTCLOUD ENCODING ***\n"); 00152 PCL_INFO ("Frame ID: %d\n", frame_ID_); 00153 if (i_frame_) 00154 PCL_INFO ("Encoding Frame: Intra frame\n"); 00155 else 00156 PCL_INFO ("Encoding Frame: Prediction frame\n"); 00157 PCL_INFO ("Number of encoded points: %ld\n", point_count_); 00158 PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof(float)) * 100.0f); 00159 PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ); 00160 PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f); 00161 PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color); 00162 PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024); 00163 PCL_INFO ("Size of compressed point cloud: %d kBytes\n", (compressed_point_data_len_ + compressed_color_data_len_) / (1024)); 00164 PCL_INFO ("Total bytes per point: %f\n", bytes_per_XYZ + bytes_per_color); 00165 PCL_INFO ("Total compression percentage: %f\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof(float)) * 100.0f); 00166 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)); 00167 } 00168 } else { 00169 if (b_show_statistics_) 00170 PCL_INFO ("Info: Dropping empty point cloud\n"); 00171 this->deleteTree(); 00172 i_frame_counter_ = 0; 00173 i_frame_ = true; 00174 } 00175 } 00176 00177 ////////////////////////////////////////////////////////////////////////////////////////////// 00178 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00179 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::decodePointCloud ( 00180 std::istream& compressed_tree_data_in_arg, 00181 PointCloudPtr &cloud_arg) 00182 { 00183 00184 // synchronize to frame header 00185 syncToHeader(compressed_tree_data_in_arg); 00186 00187 // initialize octree 00188 this->switchBuffers (); 00189 this->setOutputCloud (cloud_arg); 00190 00191 // color field analysis 00192 cloud_with_color_ = false; 00193 std::vector<pcl::PCLPointField> fields; 00194 int rgba_index = -1; 00195 rgba_index = pcl::getFieldIndex (*output_, "rgb", fields); 00196 if (rgba_index == -1) 00197 rgba_index = pcl::getFieldIndex (*output_, "rgba", fields); 00198 if (rgba_index >= 0) 00199 { 00200 point_color_offset_ = static_cast<unsigned char> (fields[rgba_index].offset); 00201 cloud_with_color_ = true; 00202 } 00203 00204 // read header from input stream 00205 this->readFrameHeader (compressed_tree_data_in_arg); 00206 00207 // decode data vectors from stream 00208 this->entropyDecoding (compressed_tree_data_in_arg); 00209 00210 // initialize color and point encoding 00211 color_coder_.initializeDecoding (); 00212 point_coder_.initializeDecoding (); 00213 00214 // initialize output cloud 00215 output_->points.clear (); 00216 output_->points.reserve (static_cast<std::size_t> (point_count_)); 00217 00218 if (i_frame_) 00219 // i-frame decoding - decode tree structure without referencing previous buffer 00220 this->deserializeTree (binary_tree_data_vector_, false); 00221 else 00222 // p-frame decoding - decode XOR encoded tree structure 00223 this->deserializeTree (binary_tree_data_vector_, true); 00224 00225 // assign point cloud properties 00226 output_->height = 1; 00227 output_->width = static_cast<uint32_t> (cloud_arg->points.size ()); 00228 output_->is_dense = false; 00229 00230 if (b_show_statistics_) 00231 { 00232 float bytes_per_XYZ = static_cast<float> (compressed_point_data_len_) / static_cast<float> (point_count_); 00233 float bytes_per_color = static_cast<float> (compressed_color_data_len_) / static_cast<float> (point_count_); 00234 00235 PCL_INFO ("*** POINTCLOUD DECODING ***\n"); 00236 PCL_INFO ("Frame ID: %d\n", frame_ID_); 00237 if (i_frame_) 00238 PCL_INFO ("Encoding Frame: Intra frame\n"); 00239 else 00240 PCL_INFO ("Encoding Frame: Prediction frame\n"); 00241 PCL_INFO ("Number of encoded points: %ld\n", point_count_); 00242 PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof (float)) * 100.0f); 00243 PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ); 00244 PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f); 00245 PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color); 00246 PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f); 00247 PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast<float> (compressed_point_data_len_ + compressed_color_data_len_) / 1024.0f); 00248 PCL_INFO ("Total bytes per point: %d bytes\n", static_cast<int> (bytes_per_XYZ + bytes_per_color)); 00249 PCL_INFO ("Total compression percentage: %f%%\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f); 00250 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)); 00251 } 00252 } 00253 00254 ////////////////////////////////////////////////////////////////////////////////////////////// 00255 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00256 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyEncoding (std::ostream& compressed_tree_data_out_arg) 00257 { 00258 uint64_t binary_tree_data_vector_size; 00259 uint64_t point_avg_color_data_vector_size; 00260 00261 compressed_point_data_len_ = 0; 00262 compressed_color_data_len_ = 0; 00263 00264 // encode binary octree structure 00265 binary_tree_data_vector_size = binary_tree_data_vector_.size (); 00266 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size)); 00267 compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (binary_tree_data_vector_, 00268 compressed_tree_data_out_arg); 00269 00270 if (cloud_with_color_) 00271 { 00272 // encode averaged voxel color information 00273 std::vector<char>& pointAvgColorDataVector = color_coder_.getAverageDataVector (); 00274 point_avg_color_data_vector_size = pointAvgColorDataVector.size (); 00275 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_avg_color_data_vector_size), 00276 sizeof (point_avg_color_data_vector_size)); 00277 compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (pointAvgColorDataVector, 00278 compressed_tree_data_out_arg); 00279 } 00280 00281 if (!do_voxel_grid_enDecoding_) 00282 { 00283 uint64_t pointCountDataVector_size; 00284 uint64_t point_diff_data_vector_size; 00285 uint64_t point_diff_color_data_vector_size; 00286 00287 // encode amount of points per voxel 00288 pointCountDataVector_size = point_count_data_vector_.size (); 00289 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size)); 00290 compressed_point_data_len_ += entropy_coder_.encodeIntVectorToStream (point_count_data_vector_, 00291 compressed_tree_data_out_arg); 00292 00293 // encode differential point information 00294 std::vector<char>& point_diff_data_vector = point_coder_.getDifferentialDataVector (); 00295 point_diff_data_vector_size = point_diff_data_vector.size (); 00296 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size)); 00297 compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_data_vector, 00298 compressed_tree_data_out_arg); 00299 if (cloud_with_color_) 00300 { 00301 // encode differential color information 00302 std::vector<char>& point_diff_color_data_vector = color_coder_.getDifferentialDataVector (); 00303 point_diff_color_data_vector_size = point_diff_color_data_vector.size (); 00304 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_diff_color_data_vector_size), 00305 sizeof (point_diff_color_data_vector_size)); 00306 compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_color_data_vector, 00307 compressed_tree_data_out_arg); 00308 } 00309 } 00310 // flush output stream 00311 compressed_tree_data_out_arg.flush (); 00312 } 00313 00314 ////////////////////////////////////////////////////////////////////////////////////////////// 00315 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00316 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyDecoding (std::istream& compressed_tree_data_in_arg) 00317 { 00318 uint64_t binary_tree_data_vector_size; 00319 uint64_t point_avg_color_data_vector_size; 00320 00321 compressed_point_data_len_ = 0; 00322 compressed_color_data_len_ = 0; 00323 00324 // decode binary octree structure 00325 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size)); 00326 binary_tree_data_vector_.resize (static_cast<std::size_t> (binary_tree_data_vector_size)); 00327 compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, 00328 binary_tree_data_vector_); 00329 00330 if (data_with_color_) 00331 { 00332 // decode averaged voxel color information 00333 std::vector<char>& point_avg_color_data_vector = color_coder_.getAverageDataVector (); 00334 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_avg_color_data_vector_size), sizeof (point_avg_color_data_vector_size)); 00335 point_avg_color_data_vector.resize (static_cast<std::size_t> (point_avg_color_data_vector_size)); 00336 compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, 00337 point_avg_color_data_vector); 00338 } 00339 00340 if (!do_voxel_grid_enDecoding_) 00341 { 00342 uint64_t point_count_data_vector_size; 00343 uint64_t point_diff_data_vector_size; 00344 uint64_t point_diff_color_data_vector_size; 00345 00346 // decode amount of points per voxel 00347 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_count_data_vector_size), sizeof (point_count_data_vector_size)); 00348 point_count_data_vector_.resize (static_cast<std::size_t> (point_count_data_vector_size)); 00349 compressed_point_data_len_ += entropy_coder_.decodeStreamToIntVector (compressed_tree_data_in_arg, point_count_data_vector_); 00350 point_count_data_vector_iterator_ = point_count_data_vector_.begin (); 00351 00352 // decode differential point information 00353 std::vector<char>& pointDiffDataVector = point_coder_.getDifferentialDataVector (); 00354 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size)); 00355 pointDiffDataVector.resize (static_cast<std::size_t> (point_diff_data_vector_size)); 00356 compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, 00357 pointDiffDataVector); 00358 00359 if (data_with_color_) 00360 { 00361 // decode differential color information 00362 std::vector<char>& pointDiffColorDataVector = color_coder_.getDifferentialDataVector (); 00363 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_diff_color_data_vector_size), sizeof (point_diff_color_data_vector_size)); 00364 pointDiffColorDataVector.resize (static_cast<std::size_t> (point_diff_color_data_vector_size)); 00365 compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, 00366 pointDiffColorDataVector); 00367 } 00368 } 00369 } 00370 00371 ////////////////////////////////////////////////////////////////////////////////////////////// 00372 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00373 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::writeFrameHeader (std::ostream& compressed_tree_data_out_arg) 00374 { 00375 // encode header identifier 00376 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (frame_header_identifier_), strlen (frame_header_identifier_)); 00377 // encode point cloud header id 00378 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&frame_ID_), sizeof (frame_ID_)); 00379 // encode frame type (I/P-frame) 00380 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&i_frame_), sizeof (i_frame_)); 00381 if (i_frame_) 00382 { 00383 double min_x, min_y, min_z, max_x, max_y, max_z; 00384 double octree_resolution; 00385 unsigned char color_bit_depth; 00386 double point_resolution; 00387 00388 // get current configuration 00389 octree_resolution = this->getResolution (); 00390 color_bit_depth = color_coder_.getBitDepth (); 00391 point_resolution= point_coder_.getPrecision (); 00392 this->getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); 00393 00394 // encode amount of points 00395 if (do_voxel_grid_enDecoding_) 00396 point_count_ = this->leaf_count_; 00397 else 00398 point_count_ = this->object_count_; 00399 00400 // encode coding configuration 00401 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_)); 00402 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&cloud_with_color_), sizeof (cloud_with_color_)); 00403 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_count_), sizeof (point_count_)); 00404 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&octree_resolution), sizeof (octree_resolution)); 00405 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&color_bit_depth), sizeof (color_bit_depth)); 00406 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&point_resolution), sizeof (point_resolution)); 00407 00408 // encode octree bounding box 00409 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&min_x), sizeof (min_x)); 00410 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&min_y), sizeof (min_y)); 00411 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&min_z), sizeof (min_z)); 00412 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&max_x), sizeof (max_x)); 00413 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&max_y), sizeof (max_y)); 00414 compressed_tree_data_out_arg.write (reinterpret_cast<const char*> (&max_z), sizeof (max_z)); 00415 } 00416 } 00417 00418 ////////////////////////////////////////////////////////////////////////////////////////////// 00419 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00420 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::syncToHeader ( std::istream& compressed_tree_data_in_arg) 00421 { 00422 // sync to frame header 00423 unsigned int header_id_pos = 0; 00424 while (header_id_pos < strlen (frame_header_identifier_)) 00425 { 00426 char readChar; 00427 compressed_tree_data_in_arg.read (static_cast<char*> (&readChar), sizeof (readChar)); 00428 if (readChar != frame_header_identifier_[header_id_pos++]) 00429 header_id_pos = (frame_header_identifier_[0]==readChar)?1:0; 00430 } 00431 } 00432 00433 ////////////////////////////////////////////////////////////////////////////////////////////// 00434 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00435 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::readFrameHeader ( std::istream& compressed_tree_data_in_arg) 00436 { 00437 // read header 00438 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&frame_ID_), sizeof (frame_ID_)); 00439 compressed_tree_data_in_arg.read (reinterpret_cast<char*>(&i_frame_), sizeof (i_frame_)); 00440 if (i_frame_) 00441 { 00442 double min_x, min_y, min_z, max_x, max_y, max_z; 00443 double octree_resolution; 00444 unsigned char color_bit_depth; 00445 double point_resolution; 00446 00447 // read coder configuration 00448 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_)); 00449 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&data_with_color_), sizeof (data_with_color_)); 00450 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_count_), sizeof (point_count_)); 00451 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&octree_resolution), sizeof (octree_resolution)); 00452 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&color_bit_depth), sizeof (color_bit_depth)); 00453 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&point_resolution), sizeof (point_resolution)); 00454 00455 // read octree bounding box 00456 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&min_x), sizeof (min_x)); 00457 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&min_y), sizeof (min_y)); 00458 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&min_z), sizeof (min_z)); 00459 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&max_x), sizeof (max_x)); 00460 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&max_y), sizeof (max_y)); 00461 compressed_tree_data_in_arg.read (reinterpret_cast<char*> (&max_z), sizeof (max_z)); 00462 00463 // reset octree and assign new bounding box & resolution 00464 this->deleteTree (); 00465 this->setResolution (octree_resolution); 00466 this->defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); 00467 00468 // configure color & point coding 00469 color_coder_.setBitDepth (color_bit_depth); 00470 point_coder_.setPrecision (static_cast<float> (point_resolution)); 00471 } 00472 } 00473 00474 ////////////////////////////////////////////////////////////////////////////////////////////// 00475 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00476 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::serializeTreeCallback ( 00477 LeafT &leaf_arg, const OctreeKey & key_arg) 00478 { 00479 // reference to point indices vector stored within octree leaf 00480 const std::vector<int>& leafIdx = leaf_arg.getPointIndicesVector(); 00481 00482 if (!do_voxel_grid_enDecoding_) 00483 { 00484 double lowerVoxelCorner[3]; 00485 00486 // encode amount of points within voxel 00487 point_count_data_vector_.push_back (static_cast<int> (leafIdx.size ())); 00488 00489 // calculate lower voxel corner based on octree key 00490 lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->min_x_; 00491 lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->min_y_; 00492 lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->min_z_; 00493 00494 // differentially encode points to lower voxel corner 00495 point_coder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_); 00496 00497 if (cloud_with_color_) 00498 // encode color of points 00499 color_coder_.encodePoints (leafIdx, point_color_offset_, this->input_); 00500 } 00501 else 00502 { 00503 if (cloud_with_color_) 00504 // encode average color of all points within voxel 00505 color_coder_.encodeAverageOfPoints (leafIdx, point_color_offset_, this->input_); 00506 } 00507 } 00508 00509 ////////////////////////////////////////////////////////////////////////////////////////////// 00510 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void 00511 OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::deserializeTreeCallback (LeafT&, 00512 const OctreeKey& key_arg) 00513 { 00514 double lowerVoxelCorner[3]; 00515 std::size_t pointCount, i, cloudSize; 00516 PointT newPoint; 00517 00518 pointCount = 1; 00519 00520 if (!do_voxel_grid_enDecoding_) 00521 { 00522 // get current cloud size 00523 cloudSize = output_->points.size (); 00524 00525 // get amount of point to be decoded 00526 pointCount = *point_count_data_vector_iterator_; 00527 point_count_data_vector_iterator_++; 00528 00529 // increase point cloud by amount of voxel points 00530 for (i = 0; i < pointCount; i++) 00531 output_->points.push_back (newPoint); 00532 00533 // calculcate position of lower voxel corner 00534 lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->min_x_; 00535 lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->min_y_; 00536 lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->min_z_; 00537 00538 // decode differentially encoded points 00539 point_coder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount); 00540 } 00541 else 00542 { 00543 // calculate center of lower voxel corner 00544 newPoint.x = static_cast<float> ((static_cast<double> (key_arg.x) + 0.5) * this->resolution_ + this->min_x_); 00545 newPoint.y = static_cast<float> ((static_cast<double> (key_arg.y) + 0.5) * this->resolution_ + this->min_y_); 00546 newPoint.z = static_cast<float> ((static_cast<double> (key_arg.z) + 0.5) * this->resolution_ + this->min_z_); 00547 00548 // add point to point cloud 00549 output_->points.push_back (newPoint); 00550 } 00551 00552 if (cloud_with_color_) 00553 { 00554 if (data_with_color_) 00555 // decode color information 00556 color_coder_.decodePoints (output_, output_->points.size () - pointCount, 00557 output_->points.size (), point_color_offset_); 00558 else 00559 // set default color information 00560 color_coder_.setDefaultColor (output_, output_->points.size () - pointCount, 00561 output_->points.size (), point_color_offset_); 00562 } 00563 } 00564 } 00565 } 00566 00567 #endif 00568