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
00039 #ifndef OCTREE_POINTCLOUD_HPP_
00040 #define OCTREE_POINTCLOUD_HPP_
00041
00042 #include <vector>
00043 #include <assert.h>
00044
00045 #include <pcl/common/common.h>
00046
00048 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT>
00049 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::OctreePointCloud (const double resolution) :
00050 OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()),
00051 epsilon_ (0), resolution_ (resolution), minX_ (0.0f), maxX_ (resolution), minY_ (0.0f),
00052 maxY_ (resolution), minZ_ (0.0f), maxZ_ (resolution), boundingBoxDefined_ (false)
00053 {
00054 assert (resolution > 0.0f);
00055 }
00056
00058 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT>
00059 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::~OctreePointCloud ()
00060 {
00061 }
00062
00064 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00065 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointsFromInputCloud ()
00066 {
00067 size_t i;
00068
00069 assert (this->leafCount_==0);
00070 if (indices_)
00071 {
00072 for (std::vector<int>::const_iterator current = indices_->begin (); current != indices_->end (); ++current)
00073 {
00074 if (isFinite (input_->points[*current]))
00075 {
00076 assert( (*current>=0) && (*current < static_cast<int> (input_->points.size ())));
00077
00078
00079 this->addPointIdx (*current);
00080 }
00081 }
00082 }
00083 else
00084 {
00085 for (i = 0; i < input_->points.size (); i++)
00086 {
00087 if (isFinite (input_->points[i]))
00088 {
00089
00090 this->addPointIdx (static_cast<unsigned int> (i));
00091 }
00092 }
00093 }
00094 }
00095
00097 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00098 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg)
00099 {
00100 this->addPointIdx (pointIdx_arg);
00101 if (indices_arg)
00102 indices_arg->push_back (pointIdx_arg);
00103 }
00104
00106 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00107 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg)
00108 {
00109 assert (cloud_arg==input_);
00110
00111 cloud_arg->push_back (point_arg);
00112
00113 this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
00114 }
00115
00117 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00118 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg,
00119 IndicesPtr indices_arg)
00120 {
00121 assert (cloud_arg==input_);
00122 assert (indices_arg==indices_);
00123
00124 cloud_arg->push_back (point_arg);
00125
00126 this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
00127 }
00128
00130 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> bool
00131 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::isVoxelOccupiedAtPoint (const PointT& point_arg) const
00132 {
00133 OctreeKey key;
00134
00135
00136 this->genOctreeKeyforPoint (point_arg, key);
00137
00138
00139 return (isPointWithinBoundingBox (point_arg) && this->existLeaf (key));
00140 }
00141
00143 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> bool
00144 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::isVoxelOccupiedAtPoint (const int& pointIdx_arg) const
00145 {
00146
00147 const PointT& point = this->input_->points[pointIdx_arg];
00148
00149
00150 return (this->isVoxelOccupiedAtPoint (point));
00151 }
00152
00154 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> bool
00155 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::isVoxelOccupiedAtPoint (
00156 const double pointX_arg, const double pointY_arg, const double pointZ_arg) const
00157 {
00158 OctreeKey key;
00159
00160
00161 this->genOctreeKeyforPoint (pointX_arg, pointY_arg, pointZ_arg, key);
00162
00163 return (this->existLeaf (key));
00164 }
00165
00167 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00168 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::deleteVoxelAtPoint (const PointT& point_arg)
00169 {
00170 OctreeKey key;
00171
00172
00173 this->genOctreeKeyforPoint (point_arg, key);
00174
00175 this->removeLeaf (key);
00176 }
00177
00179 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00180 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::deleteVoxelAtPoint (const int& pointIdx_arg)
00181 {
00182
00183 const PointT& point = this->input_->points[pointIdx_arg];
00184
00185
00186 this->deleteVoxelAtPoint (point);
00187 }
00188
00190 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
00191 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getOccupiedVoxelCenters (
00192 AlignedPointTVector &voxelCenterList_arg) const
00193 {
00194 OctreeKey key;
00195 key.x = key.y = key.z = 0;
00196
00197 voxelCenterList_arg.clear ();
00198
00199 return getOccupiedVoxelCentersRecursive (this->rootNode_, key, voxelCenterList_arg);
00200
00201 }
00202
00204 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
00205 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
00206 const Eigen::Vector3f& origin,
00207 const Eigen::Vector3f& end,
00208 AlignedPointTVector &voxel_center_list,
00209 float precision)
00210 {
00211 Eigen::Vector3f direction = end - origin;
00212 float norm = direction.norm ();
00213 direction.normalize ();
00214
00215 const float step_size = static_cast<const float> (resolution_) * precision;
00216
00217 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
00218
00219 OctreeKey prev_key;
00220
00221 bool bkeyDefined = false;
00222
00223
00224 for (int i = 0; i < nsteps; ++i)
00225 {
00226 Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
00227
00228 PointT octree_p;
00229 octree_p.x = p.x ();
00230 octree_p.y = p.y ();
00231 octree_p.z = p.z ();
00232
00233 OctreeKey key;
00234 this->genOctreeKeyforPoint (octree_p, key);
00235
00236
00237 if ((key == prev_key) && (bkeyDefined) )
00238 continue;
00239
00240 prev_key = key;
00241 bkeyDefined = true;
00242
00243 PointT center;
00244 genLeafNodeCenterFromOctreeKey (key, center);
00245 voxel_center_list.push_back (center);
00246 }
00247
00248 OctreeKey end_key;
00249 PointT end_p;
00250 end_p.x = end.x ();
00251 end_p.y = end.y ();
00252 end_p.z = end.z ();
00253 this->genOctreeKeyforPoint (end_p, end_key);
00254 if (!(end_key == prev_key))
00255 {
00256 PointT center;
00257 genLeafNodeCenterFromOctreeKey (end_key, center);
00258 voxel_center_list.push_back (center);
00259 }
00260
00261 return (static_cast<int> (voxel_center_list.size ()));
00262 }
00263
00265 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00266 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::defineBoundingBox ()
00267 {
00268
00269 double minX, minY, minZ, maxX, maxY, maxZ;
00270
00271 PointT min_pt;
00272 PointT max_pt;
00273
00274
00275 assert (this->leafCount_ == 0);
00276
00277 pcl::getMinMax3D (*input_, min_pt, max_pt);
00278
00279 float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
00280
00281 minX = min_pt.x;
00282 minY = min_pt.y;
00283 minZ = min_pt.z;
00284
00285 maxX = max_pt.x + minValue;
00286 maxY = max_pt.y + minValue;
00287 maxZ = max_pt.z + minValue;
00288
00289
00290 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00291 }
00292
00294 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00295 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::defineBoundingBox (const double minX_arg,
00296 const double minY_arg,
00297 const double minZ_arg,
00298 const double maxX_arg,
00299 const double maxY_arg,
00300 const double maxZ_arg)
00301 {
00302
00303 assert (this->leafCount_ == 0);
00304
00305 assert (maxX_arg >= minX_arg);
00306 assert (maxY_arg >= minY_arg);
00307 assert (maxZ_arg >= minZ_arg);
00308
00309 minX_ = minX_arg;
00310 maxX_ = maxX_arg;
00311
00312 minY_ = minY_arg;
00313 maxY_ = maxY_arg;
00314
00315 minZ_ = minZ_arg;
00316 maxZ_ = maxZ_arg;
00317
00318 minX_ = min (minX_, maxX_);
00319 minY_ = min (minY_, maxY_);
00320 minZ_ = min (minZ_, maxZ_);
00321
00322 maxX_ = max (minX_, maxX_);
00323 maxY_ = max (minY_, maxY_);
00324 maxZ_ = max (minZ_, maxZ_);
00325
00326
00327 getKeyBitSize ();
00328
00329 boundingBoxDefined_ = true;
00330 }
00331
00333 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00334 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::defineBoundingBox (
00335 const double maxX_arg, const double maxY_arg, const double maxZ_arg)
00336 {
00337
00338 assert (this->leafCount_ == 0);
00339
00340 assert (maxX_arg >= 0.0f);
00341 assert (maxY_arg >= 0.0f);
00342 assert (maxZ_arg >= 0.0f);
00343
00344 minX_ = 0.0f;
00345 maxX_ = maxX_arg;
00346
00347 minY_ = 0.0f;
00348 maxY_ = maxY_arg;
00349
00350 minZ_ = 0.0f;
00351 maxZ_ = maxZ_arg;
00352
00353 minX_ = min (minX_, maxX_);
00354 minY_ = min (minY_, maxY_);
00355 minZ_ = min (minZ_, maxZ_);
00356
00357 maxX_ = max (minX_, maxX_);
00358 maxY_ = max (minY_, maxY_);
00359 maxZ_ = max (minZ_, maxZ_);
00360
00361
00362 getKeyBitSize ();
00363
00364 boundingBoxDefined_ = true;
00365 }
00366
00368 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00369 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::defineBoundingBox (const double cubeLen_arg)
00370 {
00371
00372 assert (this->leafCount_ == 0);
00373
00374 assert (cubeLen_arg >= 0.0f);
00375
00376 minX_ = 0.0f;
00377 maxX_ = cubeLen_arg;
00378
00379 minY_ = 0.0f;
00380 maxY_ = cubeLen_arg;
00381
00382 minZ_ = 0.0f;
00383 maxZ_ = cubeLen_arg;
00384
00385 minX_ = min (minX_, maxX_);
00386 minY_ = min (minY_, maxY_);
00387 minZ_ = min (minZ_, maxZ_);
00388
00389 maxX_ = max (minX_, maxX_);
00390 maxY_ = max (minY_, maxY_);
00391 maxZ_ = max (minZ_, maxZ_);
00392
00393
00394 getKeyBitSize ();
00395
00396 boundingBoxDefined_ = true;
00397 }
00398
00400 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00401 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getBoundingBox (
00402 double& minX_arg, double& minY_arg, double& minZ_arg,
00403 double& maxX_arg, double& maxY_arg, double& maxZ_arg) const
00404 {
00405 minX_arg = minX_;
00406 minY_arg = minY_;
00407 minZ_arg = minZ_;
00408
00409 maxX_arg = maxX_;
00410 maxY_arg = maxY_;
00411 maxZ_arg = maxZ_;
00412 }
00413
00415 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT>
00416 void
00417 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::adoptBoundingBoxToPoint (const PointT& pointIdx_arg)
00418 {
00419
00420 const float minValue = std::numeric_limits<float>::epsilon ();
00421
00422
00423 while (true)
00424 {
00425 bool bLowerBoundViolationX = (pointIdx_arg.x < minX_);
00426 bool bLowerBoundViolationY = (pointIdx_arg.y < minY_);
00427 bool bLowerBoundViolationZ = (pointIdx_arg.z < minZ_);
00428
00429 bool bUpperBoundViolationX = (pointIdx_arg.x >= maxX_);
00430 bool bUpperBoundViolationY = (pointIdx_arg.y >= maxY_);
00431 bool bUpperBoundViolationZ = (pointIdx_arg.z >= maxZ_);
00432
00433
00434 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
00435 || bUpperBoundViolationY || bUpperBoundViolationZ )
00436 {
00437
00438 if (boundingBoxDefined_)
00439 {
00440
00441 double octreeSideLen;
00442 unsigned char childIdx;
00443
00444
00445 childIdx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
00446 | ((!bUpperBoundViolationZ)));
00447
00448 BranchNode* newRootBranch;
00449
00450 newRootBranch = this->branchNodePool_.popNode();
00451 this->branchCount_++;
00452
00453 this->setBranchChildPtr (*newRootBranch, childIdx, this->rootNode_);
00454
00455 this->rootNode_ = newRootBranch;
00456
00457 octreeSideLen = static_cast<double> (1 << this->octreeDepth_) * resolution_;
00458
00459 if (!bUpperBoundViolationX)
00460 minX_ -= octreeSideLen;
00461
00462 if (!bUpperBoundViolationY)
00463 minY_ -= octreeSideLen;
00464
00465 if (!bUpperBoundViolationZ)
00466 minZ_ -= octreeSideLen;
00467
00468
00469 this->octreeDepth_++;
00470 this->setTreeDepth (this->octreeDepth_);
00471
00472
00473 octreeSideLen = static_cast<double> (1 << this->octreeDepth_) * resolution_ - minValue;
00474
00475
00476 maxX_ = minX_ + octreeSideLen;
00477 maxY_ = minY_ + octreeSideLen;
00478 maxZ_ = minZ_ + octreeSideLen;
00479
00480 }
00481
00482 else
00483 {
00484
00485 this->minX_ = pointIdx_arg.x - this->resolution_ / 2;
00486 this->minY_ = pointIdx_arg.y - this->resolution_ / 2;
00487 this->minZ_ = pointIdx_arg.z - this->resolution_ / 2;
00488
00489 this->maxX_ = pointIdx_arg.x + this->resolution_ / 2;
00490 this->maxY_ = pointIdx_arg.y + this->resolution_ / 2;
00491 this->maxZ_ = pointIdx_arg.z + this->resolution_ / 2;
00492
00493 getKeyBitSize ();
00494
00495 boundingBoxDefined_ = true;
00496 }
00497
00498 }
00499 else
00500
00501 break;
00502 }
00503 }
00504
00506 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00507 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointIdx (const int pointIdx_arg)
00508 {
00509 OctreeKey key;
00510
00511 assert (pointIdx_arg < static_cast<int> (input_->points.size ()));
00512
00513 const PointT& point = input_->points[pointIdx_arg];
00514
00515
00516 adoptBoundingBoxToPoint (point);
00517
00518
00519 genOctreeKeyforPoint (point, key);
00520
00521
00522 this->addData (key, pointIdx_arg);
00523 }
00524
00526 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> const PointT&
00527 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getPointByIndex (const unsigned int index_arg) const
00528 {
00529
00530 assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
00531 return (this->input_->points[index_arg]);
00532 }
00533
00535 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> LeafT*
00536 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::findLeafAtPoint (const PointT& point) const
00537 {
00538 OctreeKey key;
00539
00540
00541 this->genOctreeKeyforPoint (point, key);
00542
00543 return (this->findLeaf (key));
00544 }
00545
00547 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00548 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getKeyBitSize ()
00549 {
00550 unsigned int maxVoxels;
00551
00552 unsigned int maxKeyX;
00553 unsigned int maxKeyY;
00554 unsigned int maxKeyZ;
00555
00556 double octreeSideLen;
00557
00558 const float minValue = std::numeric_limits<float>::epsilon();
00559
00560
00561 maxKeyX = static_cast<unsigned int> ((maxX_ - minX_) / resolution_);
00562 maxKeyY = static_cast<unsigned int> ((maxY_ - minY_) / resolution_);
00563 maxKeyZ = static_cast<unsigned int> ((maxZ_ - minZ_) / resolution_);
00564
00565
00566 maxVoxels = max (max (max (maxKeyX, maxKeyY), maxKeyZ), static_cast<unsigned int> (2));
00567
00568
00569
00570 this->octreeDepth_ = max ((min (static_cast<unsigned int> (OCT_MAXTREEDEPTH), static_cast<unsigned int> (ceil (this->Log2 (maxVoxels)-minValue)))),
00571 static_cast<unsigned int> (0));
00572
00573 octreeSideLen = static_cast<double> (1 << this->octreeDepth_) * resolution_-minValue;
00574
00575 if (this->leafCount_ == 0)
00576 {
00577 double octreeOversizeX;
00578 double octreeOversizeY;
00579 double octreeOversizeZ;
00580
00581 octreeOversizeX = (octreeSideLen - (maxX_ - minX_)) / 2.0;
00582 octreeOversizeY = (octreeSideLen - (maxY_ - minY_)) / 2.0;
00583 octreeOversizeZ = (octreeSideLen - (maxZ_ - minZ_)) / 2.0;
00584
00585 minX_ -= octreeOversizeX;
00586 minY_ -= octreeOversizeY;
00587 minZ_ -= octreeOversizeZ;
00588
00589 maxX_ += octreeOversizeX;
00590 maxY_ += octreeOversizeY;
00591 maxZ_ += octreeOversizeZ;
00592 }
00593 else
00594 {
00595 maxX_ = minX_ + octreeSideLen;
00596 maxY_ = minY_ + octreeSideLen;
00597 maxZ_ = minZ_ + octreeSideLen;
00598 }
00599
00600
00601 this->setTreeDepth (this->octreeDepth_);
00602
00603 }
00604
00606 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00607 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genOctreeKeyforPoint (const PointT& point_arg,
00608 OctreeKey & key_arg) const
00609 {
00610
00611 key_arg.x = static_cast<unsigned int> ((point_arg.x - this->minX_) / this->resolution_);
00612 key_arg.y = static_cast<unsigned int> ((point_arg.y - this->minY_) / this->resolution_);
00613 key_arg.z = static_cast<unsigned int> ((point_arg.z - this->minZ_) / this->resolution_);
00614 }
00615
00617 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00618 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genOctreeKeyforPoint (
00619 const double pointX_arg, const double pointY_arg,
00620 const double pointZ_arg, OctreeKey & key_arg) const
00621 {
00622 PointT tempPoint;
00623
00624 tempPoint.x = static_cast<float> (pointX_arg);
00625 tempPoint.y = static_cast<float> (pointY_arg);
00626 tempPoint.z = static_cast<float> (pointZ_arg);
00627
00628
00629 genOctreeKeyforPoint (tempPoint, key_arg);
00630 }
00631
00633 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> bool
00634 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const
00635 {
00636 const PointT tempPoint = getPointByIndex (data_arg);
00637
00638
00639 genOctreeKeyforPoint (tempPoint, key_arg);
00640
00641 return (true);
00642 }
00643
00645 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00646 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const
00647 {
00648
00649 point.x = static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->minX_);
00650 point.y = static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->minY_);
00651 point.z = static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->minZ_);
00652 }
00653
00655 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00656 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genVoxelCenterFromOctreeKey (
00657 const OctreeKey & key_arg,
00658 unsigned int treeDepth_arg,
00659 PointT& point_arg) const
00660 {
00661
00662 point_arg.x = static_cast<float> ((static_cast <double> (key_arg.x) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octreeDepth_ - treeDepth_arg))) + this->minX_);
00663 point_arg.y = static_cast<float> ((static_cast <double> (key_arg.y) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octreeDepth_ - treeDepth_arg))) + this->minY_);
00664 point_arg.z = static_cast<float> ((static_cast <double> (key_arg.z) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octreeDepth_ - treeDepth_arg))) + this->minZ_);
00665 }
00666
00668 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
00669 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genVoxelBoundsFromOctreeKey (
00670 const OctreeKey & key_arg,
00671 unsigned int treeDepth_arg,
00672 Eigen::Vector3f &min_pt,
00673 Eigen::Vector3f &max_pt) const
00674 {
00675
00676 double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octreeDepth_ - treeDepth_arg));
00677
00678
00679 min_pt (0) = static_cast<float> (static_cast<double> (key_arg.x) * voxel_side_len + this->minX_);
00680 min_pt (1) = static_cast<float> (static_cast<double> (key_arg.y) * voxel_side_len + this->minY_);
00681 min_pt (2) = static_cast<float> (static_cast<double> (key_arg.z) * voxel_side_len + this->minZ_);
00682
00683 max_pt (0) = static_cast<float> (static_cast<double> (key_arg.x + 1) * voxel_side_len + this->minX_);
00684 max_pt (1) = static_cast<float> (static_cast<double> (key_arg.y + 1) * voxel_side_len + this->minY_);
00685 max_pt (2) = static_cast<float> (static_cast<double> (key_arg.z + 1) * voxel_side_len + this->minZ_);
00686 }
00687
00689 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> double
00690 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getVoxelSquaredSideLen (unsigned int treeDepth_arg) const
00691 {
00692 double sideLen;
00693
00694
00695 sideLen = this->resolution_ * static_cast<double>(1 << (this->octreeDepth_ - treeDepth_arg));
00696
00697
00698 sideLen *= sideLen;
00699
00700 return (sideLen);
00701 }
00702
00704 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> double
00705 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getVoxelSquaredDiameter (unsigned int treeDepth_arg) const
00706 {
00707
00708 return (getVoxelSquaredSideLen (treeDepth_arg) * 3);
00709 }
00710
00712 template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
00713 pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getOccupiedVoxelCentersRecursive (
00714 const BranchNode* node_arg,
00715 const OctreeKey& key_arg,
00716 AlignedPointTVector &voxelCenterList_arg) const
00717 {
00718
00719 unsigned char childIdx;
00720
00721 int voxelCount = 0;
00722
00723
00724 for (childIdx = 0; childIdx < 8; childIdx++)
00725 {
00726 if (!this->branchHasChild (*node_arg, childIdx))
00727 continue;
00728
00729 const OctreeNode * childNode;
00730 childNode = this->getBranchChildPtr (*node_arg, childIdx);
00731
00732
00733 OctreeKey newKey;
00734 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00735 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00736 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00737
00738 switch (childNode->getNodeType ())
00739 {
00740 case BRANCH_NODE:
00741 {
00742
00743 voxelCount += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (childNode), newKey, voxelCenterList_arg);
00744 break;
00745 }
00746 case LEAF_NODE:
00747 {
00748 PointT newPoint;
00749
00750 genLeafNodeCenterFromOctreeKey (newKey, newPoint);
00751 voxelCenterList_arg.push_back (newPoint);
00752
00753 voxelCount++;
00754 break;
00755 }
00756 default:
00757 break;
00758 }
00759 }
00760 return (voxelCount);
00761 }
00762
00763 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerDataTVector<int> , pcl::octree::OctreeContainerEmpty<int>, pcl::octree::OctreeBase<int, pcl::octree::OctreeContainerDataTVector<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00764 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerDataTVector<int> , pcl::octree::OctreeContainerEmpty<int>, pcl::octree::Octree2BufBase<int, pcl::octree::OctreeContainerDataTVector<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00765
00766 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int>, pcl::octree::OctreeContainerEmpty<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeContainerDataT<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00767 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int>, pcl::octree::OctreeContainerEmpty<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeContainerDataT<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00768
00769 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int>, pcl::octree::OctreeContainerEmpty<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeContainerDataT<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00770 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int>, pcl::octree::OctreeContainerEmpty<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeContainerDataT<int>, pcl::octree::OctreeContainerEmpty<int> > >;
00771
00772 #endif