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