00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00052 namespace pcl
00053 {
00054 namespace octree
00055 {
00057 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void PointCloudCompression<
00058 PointT, LeafT, BranchT, OctreeT>::encodePointCloud (
00059 const PointCloudConstPtr &cloud_arg,
00060 std::ostream& compressedTreeDataOut_arg)
00061 {
00062 unsigned char recentTreeDepth =
00063 static_cast<unsigned char> (this->getTreeDepth ());
00064
00065
00066 this->setInputCloud (cloud_arg);
00067
00068
00069 this->addPointsFromInputCloud ();
00070
00071
00072 if (this->leafCount_>0) {
00073
00074
00075
00076 cloudWithColor_ = false;
00077 std::vector<sensor_msgs::PointField> fields;
00078 int rgba_index = -1;
00079 rgba_index = pcl::getFieldIndex (*this->input_, "rgb", fields);
00080 if (rgba_index == -1)
00081 {
00082 rgba_index = pcl::getFieldIndex (*this->input_, "rgba", fields);
00083 }
00084 if (rgba_index >= 0)
00085 {
00086 pointColorOffset_ = static_cast<unsigned char> (fields[rgba_index].offset);
00087 cloudWithColor_ = true;
00088 }
00089
00090
00091 cloudWithColor_ &= doColorEncoding_;
00092
00093
00094
00095 iFrame_ |= (recentTreeDepth != this->getTreeDepth ());
00096
00097
00098 if (iFrameCounter_++==iFrameRate_)
00099 {
00100 iFrameCounter_ =0;
00101 iFrame_ = true;
00102 }
00103
00104
00105 frameID_++;
00106
00107
00108 if (!doVoxelGridEnDecoding_)
00109 {
00110 pointCountDataVector_.clear ();
00111 pointCountDataVector_.reserve (cloud_arg->points.size ());
00112 }
00113
00114
00115 colorCoder_.initializeEncoding ();
00116 colorCoder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
00117 colorCoder_.setVoxelCount (static_cast<unsigned int> (this->leafCount_));
00118
00119
00120 pointCoder_.initializeEncoding ();
00121 pointCoder_.setPointCount (static_cast<unsigned int> (cloud_arg->points.size ()));
00122
00123
00124 if (iFrame_)
00125
00126 this->serializeTree (binaryTreeDataVector_, false);
00127 else
00128
00129 this->serializeTree (binaryTreeDataVector_, true);
00130
00131
00132 this->writeFrameHeader (compressedTreeDataOut_arg);
00133
00134
00135 this->entropyEncoding (compressedTreeDataOut_arg);
00136
00137
00138 this->switchBuffers ();
00139 iFrame_ = false;
00140
00141 if (bShowStatistics)
00142 {
00143 float bytesPerXYZ = static_cast<float> (compressedPointDataLen_) / static_cast<float> (pointCount_);
00144 float bytesPerColor = static_cast<float> (compressedColorDataLen_) / static_cast<float> (pointCount_);
00145
00146 PCL_INFO ("*** POINTCLOUD ENCODING ***\n");
00147 PCL_INFO ("Frame ID: %d\n", frameID_);
00148 if (iFrame_)
00149 PCL_INFO ("Encoding Frame: Intra frame\n");
00150 else
00151 PCL_INFO ("Encoding Frame: Prediction frame\n");
00152 PCL_INFO ("Number of encoded points: %ld\n", pointCount_);
00153 PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof(float)) * 100.0f);
00154 PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ);
00155 PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f);
00156 PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor);
00157 PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024);
00158 PCL_INFO ("Size of compressed point cloud: %d kBytes\n", (compressedPointDataLen_ + compressedColorDataLen_) / (1024));
00159 PCL_INFO ("Total bytes per point: %f\n", bytesPerXYZ + bytesPerColor);
00160 PCL_INFO ("Total compression percentage: %f\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof(float)) * 100.0f);
00161 PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytesPerXYZ + bytesPerColor));
00162 }
00163 } else {
00164 if (bShowStatistics)
00165 PCL_INFO ("Info: Dropping empty point cloud\n");
00166 this->deleteTree();
00167 iFrameCounter_ = 0;
00168 iFrame_ = true;
00169 }
00170 }
00171
00173 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00174 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::decodePointCloud (
00175 std::istream& compressedTreeDataIn_arg,
00176 PointCloudPtr &cloud_arg)
00177 {
00178
00179
00180 syncToHeader(compressedTreeDataIn_arg);
00181
00182
00183 this->switchBuffers ();
00184 this->setOutputCloud (cloud_arg);
00185
00186
00187 cloudWithColor_ = false;
00188 std::vector<sensor_msgs::PointField> fields;
00189 int rgba_index = -1;
00190 rgba_index = pcl::getFieldIndex (*output_, "rgb", fields);
00191 if (rgba_index == -1)
00192 rgba_index = pcl::getFieldIndex (*output_, "rgba", fields);
00193 if (rgba_index >= 0)
00194 {
00195 pointColorOffset_ = static_cast<unsigned char> (fields[rgba_index].offset);
00196 cloudWithColor_ = true;
00197 }
00198
00199
00200 this->readFrameHeader (compressedTreeDataIn_arg);
00201
00202
00203 this->entropyDecoding (compressedTreeDataIn_arg);
00204
00205
00206 colorCoder_.initializeDecoding ();
00207 pointCoder_.initializeDecoding ();
00208
00209
00210 output_->points.clear ();
00211 output_->points.reserve (static_cast<std::size_t> (pointCount_));
00212
00213 if (iFrame_)
00214
00215 this->deserializeTree (binaryTreeDataVector_, false);
00216 else
00217
00218 this->deserializeTree (binaryTreeDataVector_, true);
00219
00220
00221 output_->height = 1;
00222 output_->width = static_cast<uint32_t> (cloud_arg->points.size ());
00223 output_->is_dense = false;
00224
00225 if (bShowStatistics)
00226 {
00227 float bytesPerXYZ = static_cast<float> (compressedPointDataLen_) / static_cast<float> (pointCount_);
00228 float bytesPerColor = static_cast<float> (compressedColorDataLen_) / static_cast<float> (pointCount_);
00229
00230 PCL_INFO ("*** POINTCLOUD DECODING ***\n");
00231 PCL_INFO ("Frame ID: %d\n", frameID_);
00232 if (iFrame_)
00233 PCL_INFO ("Encoding Frame: Intra frame\n");
00234 else
00235 PCL_INFO ("Encoding Frame: Prediction frame\n");
00236 PCL_INFO ("Number of encoded points: %ld\n", pointCount_);
00237 PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof (float)) * 100.0f);
00238 PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ);
00239 PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f);
00240 PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor);
00241 PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast<float> (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f);
00242 PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast<float> (compressedPointDataLen_ + compressedColorDataLen_) / 1024.0f);
00243 PCL_INFO ("Total bytes per point: %d bytes\n", static_cast<int> (bytesPerXYZ + bytesPerColor));
00244 PCL_INFO ("Total compression percentage: %f%%\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f);
00245 PCL_INFO ("Compression ratio: %f\n\n", static_cast<float> (sizeof (int) + 3.0f * sizeof (float)) / static_cast<float> (bytesPerXYZ + bytesPerColor));
00246 }
00247 }
00248
00250 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00251 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyEncoding (std::ostream& compressedTreeDataOut_arg)
00252 {
00253 uint64_t binaryTreeDataVector_size;
00254 uint64_t pointAvgColorDataVector_size;
00255
00256 compressedPointDataLen_ = 0;
00257 compressedColorDataLen_ = 0;
00258
00259
00260 binaryTreeDataVector_size = binaryTreeDataVector_.size ();
00261 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size));
00262 compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (binaryTreeDataVector_,
00263 compressedTreeDataOut_arg);
00264
00265 if (cloudWithColor_)
00266 {
00267
00268 std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
00269 pointAvgColorDataVector_size = pointAvgColorDataVector.size ();
00270 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointAvgColorDataVector_size),
00271 sizeof (pointAvgColorDataVector_size));
00272 compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointAvgColorDataVector,
00273 compressedTreeDataOut_arg);
00274 }
00275
00276 if (!doVoxelGridEnDecoding_)
00277 {
00278 uint64_t pointCountDataVector_size;
00279 uint64_t pointDiffDataVector_size;
00280 uint64_t pointDiffColorDataVector_size;
00281
00282
00283 pointCountDataVector_size = pointCountDataVector_.size ();
00284 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
00285 compressedPointDataLen_ += entropyCoder_.encodeIntVectorToStream (pointCountDataVector_,
00286 compressedTreeDataOut_arg);
00287
00288
00289 std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
00290 pointDiffDataVector_size = pointDiffDataVector.size ();
00291 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size));
00292 compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffDataVector,
00293 compressedTreeDataOut_arg);
00294 if (cloudWithColor_)
00295 {
00296
00297 std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
00298 pointDiffColorDataVector_size = pointDiffColorDataVector.size ();
00299 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointDiffColorDataVector_size),
00300 sizeof (pointDiffColorDataVector_size));
00301 compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffColorDataVector,
00302 compressedTreeDataOut_arg);
00303 }
00304 }
00305
00306 compressedTreeDataOut_arg.flush ();
00307 }
00308
00310 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00311 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::entropyDecoding (std::istream& compressedTreeDataIn_arg)
00312 {
00313 uint64_t binaryTreeDataVector_size;
00314 uint64_t pointAvgColorDataVector_size;
00315
00316 compressedPointDataLen_ = 0;
00317 compressedColorDataLen_ = 0;
00318
00319
00320 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size));
00321 binaryTreeDataVector_.resize (static_cast<std::size_t> (binaryTreeDataVector_size));
00322 compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00323 binaryTreeDataVector_);
00324
00325 if (dataWithColor_)
00326 {
00327
00328 std::vector<char>& pointAvgColorDataVector = colorCoder_.getAverageDataVector ();
00329 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointAvgColorDataVector_size), sizeof (pointAvgColorDataVector_size));
00330 pointAvgColorDataVector.resize (static_cast<std::size_t> (pointAvgColorDataVector_size));
00331 compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00332 pointAvgColorDataVector);
00333 }
00334
00335 if (!doVoxelGridEnDecoding_)
00336 {
00337 uint64_t pointCountDataVector_size;
00338 uint64_t pointDiffDataVector_size;
00339 uint64_t pointDiffColorDataVector_size;
00340
00341
00342 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointCountDataVector_size), sizeof (pointCountDataVector_size));
00343 pointCountDataVector_.resize (static_cast<std::size_t> (pointCountDataVector_size));
00344 compressedPointDataLen_ += entropyCoder_.decodeStreamToIntVector (compressedTreeDataIn_arg, pointCountDataVector_);
00345 pointCountDataVectorIterator_ = pointCountDataVector_.begin ();
00346
00347
00348 std::vector<char>& pointDiffDataVector = pointCoder_.getDifferentialDataVector ();
00349 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size));
00350 pointDiffDataVector.resize (static_cast<std::size_t> (pointDiffDataVector_size));
00351 compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00352 pointDiffDataVector);
00353
00354 if (dataWithColor_)
00355 {
00356
00357 std::vector<char>& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector ();
00358 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointDiffColorDataVector_size), sizeof (pointDiffColorDataVector_size));
00359 pointDiffColorDataVector.resize (static_cast<std::size_t> (pointDiffColorDataVector_size));
00360 compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg,
00361 pointDiffColorDataVector);
00362 }
00363 }
00364 }
00365
00367 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00368 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::writeFrameHeader (std::ostream& compressedTreeDataOut_arg)
00369 {
00370
00371 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
00372
00373 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&frameID_), sizeof (frameID_));
00374
00375 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&iFrame_), sizeof (iFrame_));
00376 if (iFrame_)
00377 {
00378 double minX, minY, minZ, maxX, maxY, maxZ;
00379 double octreeResolution;
00380 unsigned char colorBitDepth;
00381 double pointResolution;
00382
00383
00384 octreeResolution = this->getResolution ();
00385 colorBitDepth = colorCoder_.getBitDepth ();
00386 pointResolution= pointCoder_.getPrecision ();
00387 this->getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00388
00389
00390 if (doVoxelGridEnDecoding_)
00391 pointCount_ = this->leafCount_;
00392 else
00393 pointCount_ = this->objectCount_;
00394
00395
00396 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_));
00397 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&cloudWithColor_), sizeof (cloudWithColor_));
00398 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointCount_), sizeof (pointCount_));
00399 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&octreeResolution), sizeof (octreeResolution));
00400 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&colorBitDepth), sizeof (colorBitDepth));
00401 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&pointResolution), sizeof (pointResolution));
00402
00403
00404 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minX), sizeof (minX));
00405 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minY), sizeof (minY));
00406 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&minZ), sizeof (minZ));
00407 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxX), sizeof (maxX));
00408 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxY), sizeof (maxY));
00409 compressedTreeDataOut_arg.write (reinterpret_cast<const char*> (&maxZ), sizeof (maxZ));
00410 }
00411 }
00412
00414 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00415 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::syncToHeader ( std::istream& compressedTreeDataIn_arg)
00416 {
00417
00418 unsigned int headerIdPos = 0;
00419 while (headerIdPos < strlen (frameHeaderIdentifier_))
00420 {
00421 char readChar;
00422 compressedTreeDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
00423 if (readChar != frameHeaderIdentifier_[headerIdPos++])
00424 headerIdPos = (frameHeaderIdentifier_[0]==readChar)?1:0;
00425 }
00426 }
00427
00429 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00430 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::readFrameHeader ( std::istream& compressedTreeDataIn_arg)
00431 {
00432
00433 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&frameID_), sizeof (frameID_));
00434 compressedTreeDataIn_arg.read (reinterpret_cast<char*>(&iFrame_), sizeof (iFrame_));
00435 if (iFrame_)
00436 {
00437 double minX, minY, minZ, maxX, maxY, maxZ;
00438 double octreeResolution;
00439 unsigned char colorBitDepth;
00440 double pointResolution;
00441
00442
00443 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_));
00444 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&dataWithColor_), sizeof (dataWithColor_));
00445 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointCount_), sizeof (pointCount_));
00446 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&octreeResolution), sizeof (octreeResolution));
00447 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&colorBitDepth), sizeof (colorBitDepth));
00448 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&pointResolution), sizeof (pointResolution));
00449
00450
00451 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minX), sizeof (minX));
00452 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minY), sizeof (minY));
00453 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&minZ), sizeof (minZ));
00454 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxX), sizeof (maxX));
00455 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxY), sizeof (maxY));
00456 compressedTreeDataIn_arg.read (reinterpret_cast<char*> (&maxZ), sizeof (maxZ));
00457
00458
00459 this->deleteTree ();
00460 this->setResolution (octreeResolution);
00461 this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00462
00463
00464 colorCoder_.setBitDepth (colorBitDepth);
00465 pointCoder_.setPrecision (static_cast<float> (pointResolution));
00466 }
00467 }
00468
00470 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00471 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::serializeTreeCallback (
00472 LeafNode &leaf_arg, const OctreeKey & key_arg)
00473 {
00474
00475 const std::vector<int>& leafIdx = leaf_arg.getDataTVector ();
00476
00477 if (!doVoxelGridEnDecoding_)
00478 {
00479 double lowerVoxelCorner[3];
00480
00481
00482 pointCountDataVector_.push_back (static_cast<int> (leafIdx.size ()));
00483
00484
00485 lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->minX_;
00486 lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->minY_;
00487 lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->minZ_;
00488
00489
00490 pointCoder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_);
00491
00492 if (cloudWithColor_)
00493
00494 colorCoder_.encodePoints (leafIdx, pointColorOffset_, this->input_);
00495 }
00496 else
00497 {
00498 if (cloudWithColor_)
00499
00500 colorCoder_.encodeAverageOfPoints (leafIdx, pointColorOffset_, this->input_);
00501 }
00502 }
00503
00505 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00506 PointCloudCompression<PointT, LeafT, BranchT, OctreeT>::deserializeTreeCallback (LeafNode&,
00507 const OctreeKey& key_arg)
00508 {
00509 double lowerVoxelCorner[3];
00510 std::size_t pointCount, i, cloudSize;
00511 PointT newPoint;
00512
00513 pointCount = 1;
00514
00515 if (!doVoxelGridEnDecoding_)
00516 {
00517
00518 cloudSize = output_->points.size ();
00519
00520
00521 pointCount = *pointCountDataVectorIterator_;
00522 pointCountDataVectorIterator_++;
00523
00524
00525 for (i = 0; i < pointCount; i++)
00526 output_->points.push_back (newPoint);
00527
00528
00529 lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->minX_;
00530 lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->minY_;
00531 lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->minZ_;
00532
00533
00534 pointCoder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount);
00535 }
00536 else
00537 {
00538
00539 newPoint.x = static_cast<float> ((static_cast<double> (key_arg.x) + 0.5) * this->resolution_ + this->minX_);
00540 newPoint.y = static_cast<float> ((static_cast<double> (key_arg.y) + 0.5) * this->resolution_ + this->minY_);
00541 newPoint.z = static_cast<float> ((static_cast<double> (key_arg.z) + 0.5) * this->resolution_ + this->minZ_);
00542
00543
00544 output_->points.push_back (newPoint);
00545 }
00546
00547 if (cloudWithColor_)
00548 {
00549 if (dataWithColor_)
00550
00551 colorCoder_.decodePoints (output_, output_->points.size () - pointCount,
00552 output_->points.size (), pointColorOffset_);
00553 else
00554
00555 colorCoder_.setDefaultColor (output_, output_->points.size () - pointCount,
00556 output_->points.size (), pointColorOffset_);
00557 }
00558 }
00559 }
00560 }
00561
00562 #define PCL_INSTANTIATE_PointCloudCompression(T) template class PCL_EXPORTS pcl::octree::PointCloudCompression<T, pcl::octree::OctreeContainerDataTVector<int>, pcl::octree::OctreeContainerEmpty<int>, pcl::octree::Octree2BufBase<int, pcl::octree::OctreeContainerDataTVector<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00563
00564 #endif
00565