Point Cloud Library (PCL)  1.3.1
octree_pointcloud.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, 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  * $Id: octree_pointcloud.hpp 3017 2011-11-01 03:24:04Z rusu $
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 
00047 namespace pcl
00048 {
00049   namespace octree
00050   {
00051 
00052     using namespace std;
00053 
00055     template<typename PointT, typename LeafT, typename OctreeT>
00056       OctreePointCloud<PointT, LeafT, OctreeT>::OctreePointCloud (const double resolution) :
00057         OctreeT (), epsilon_ (0), resolution_ (resolution), minX_ (0.0f), maxX_ (resolution), minY_ (0.0f),
00058             maxY_ (resolution), minZ_ (0.0f), maxZ_ (resolution), maxKeys_ (1), boundingBoxDefined_ (false)
00059       {
00060         assert ( resolution > 0.0f );
00061         input_ = PointCloudConstPtr ();
00062       }
00063 
00065     template<typename PointT, typename LeafT, typename OctreeT>
00066       OctreePointCloud<PointT, LeafT, OctreeT>::~OctreePointCloud ()
00067       {
00068 
00069       }
00070 
00072     template<typename PointT, typename LeafT, typename OctreeT>
00073       void
00074       OctreePointCloud<PointT, LeafT, OctreeT>::addPointsFromInputCloud ()
00075       {
00076         size_t i;
00077 
00078         assert (this->leafCount_==0);
00079 
00080         {
00081           if (indices_)
00082           {
00083             std::vector<int>::const_iterator current = indices_->begin ();
00084             while (current != indices_->end ())
00085             {
00086               if ((input_->points[*current].x == input_->points[*current].x) && (input_->points[*current].y
00087                   == input_->points[*current].y) && (input_->points[*current].z == input_->points[*current].z))
00088               {
00089                 // add points to octree
00090                 this->addPointIdx (*current);
00091                 ++current;
00092               }
00093             }
00094           }
00095           else
00096           {
00097             for (i = 0; i < input_->points.size (); i++)
00098             {
00099               if ((input_->points[i].x == input_->points[i].x) && (input_->points[i].y == input_->points[i].y)
00100                   && (input_->points[i].z == input_->points[i].z))
00101               {
00102                 // add points to octree
00103                 this->addPointIdx ((unsigned int)i);
00104               }
00105             }
00106           }
00107 
00108         }
00109       }
00110 
00112     template<typename PointT, typename LeafT, typename OctreeT>
00113       void
00114       OctreePointCloud<PointT, LeafT, OctreeT>::addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg)
00115       {
00116 
00117         this->addPointIdx (pointIdx_arg);
00118 
00119         if (indices_arg)
00120         {
00121           indices_arg->push_back (pointIdx_arg);
00122         }
00123 
00124       }
00125 
00127     template<typename PointT, typename LeafT, typename OctreeT>
00128       void
00129       OctreePointCloud<PointT, LeafT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg)
00130       {
00131         assert (cloud_arg==input_);
00132 
00133         cloud_arg->points.push_back (point_arg);
00134 
00135         this->addPointIdx (cloud_arg->points.size () - 1);
00136 
00137       }
00138 
00139     template<typename PointT, typename LeafT, typename OctreeT>
00140       void
00141       OctreePointCloud<PointT, LeafT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg,
00142                                                                  IndicesPtr indices_arg)
00143       {
00144 
00145         assert (cloud_arg==input_);
00146         assert (indices_arg==indices_);
00147 
00148         cloud_arg->points.push_back (point_arg);
00149 
00150         this->addPointFromCloud (cloud_arg->points.size () - 1, indices_arg);
00151 
00152       }
00153 
00155     template<typename PointT, typename LeafT, typename OctreeT>
00156       bool
00157       OctreePointCloud<PointT, LeafT, OctreeT>::isVoxelOccupiedAtPoint (const PointT& point_arg) const
00158       {
00159         OctreeKey key;
00160 
00161         // generate key for point
00162         this->genOctreeKeyforPoint (point_arg, key);
00163 
00164         // search for key in octree
00165         return this->existLeaf (key);
00166 
00167       }
00168 
00170     template<typename PointT, typename LeafT, typename OctreeT>
00171       bool
00172       OctreePointCloud<PointT, LeafT, OctreeT>::isVoxelOccupiedAtPoint (const int& pointIdx_arg) const
00173       {
00174 
00175         // retrieve point from input cloud
00176         const PointT& point = this->input_->points[pointIdx_arg];
00177 
00178         // search for voxel at point in octree
00179         return this->isVoxelOccupiedAtPoint (point);
00180 
00181       }
00182 
00184     template<typename PointT, typename LeafT, typename OctreeT>
00185       bool
00186       OctreePointCloud<PointT, LeafT, OctreeT>::isVoxelOccupiedAtPoint (const double pointX_arg,
00187                                                                         const double pointY_arg,
00188                                                                         const double pointZ_arg) const
00189       {
00190         OctreeKey key;
00191 
00192         // generate key for point
00193         this->genOctreeKeyforPoint (pointX_arg, pointY_arg, pointZ_arg, key);
00194 
00195         return this->existLeaf (key);
00196       }
00197 
00199     template<typename PointT, typename LeafT, typename OctreeT>
00200       void
00201       OctreePointCloud<PointT, LeafT, OctreeT>::deleteVoxelAtPoint (const PointT& point_arg)
00202       {
00203         OctreeKey key;
00204 
00205         // generate key for point
00206         this->genOctreeKeyforPoint (point_arg, key);
00207 
00208         this->removeLeaf (key);
00209       }
00210 
00212     template<typename PointT, typename LeafT, typename OctreeT>
00213       void
00214       OctreePointCloud<PointT, LeafT, OctreeT>::deleteVoxelAtPoint (const int& pointIdx_arg)
00215       {
00216         // retrieve point from input cloud
00217         const PointT& point = this->input_->points[pointIdx_arg];
00218 
00219         // delete leaf at point
00220         this->deleteVoxelAtPoint (point);
00221 
00222       }
00223 
00225     template<typename PointT, typename LeafT, typename OctreeT>
00226       int
00227       OctreePointCloud<PointT, LeafT, OctreeT>::getOccupiedVoxelCenters (
00228                                                                          std::vector<PointT, Eigen::aligned_allocator<
00229                                                                              PointT> > &voxelCenterList_arg) const
00230       {
00231         OctreeKey key;
00232         key.x = key.y = key.z = 0;
00233 
00234         voxelCenterList_arg.clear ();
00235         voxelCenterList_arg.reserve (this->leafCount_);
00236 
00237         return getOccupiedVoxelCentersRecursive (this->rootNode_, key, voxelCenterList_arg);
00238 
00239       }
00240 
00242     template<typename PointT, typename LeafT, typename OctreeT>
00243       void
00244       OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox ()
00245       {
00246 
00247         double minX, minY, minZ, maxX, maxY, maxZ;
00248 
00249         PointT min_pt;
00250         PointT max_pt;
00251 
00252         // bounding box cannot be changed once the octree contains elements
00253         assert ( this->leafCount_ == 0 );
00254 
00255         pcl::getMinMax3D (*input_, min_pt, max_pt);
00256 
00257         minX = min_pt.x;
00258         minY = min_pt.y;
00259         minZ = min_pt.z;
00260 
00261         maxX = max_pt.x;
00262         maxY = max_pt.y;
00263         maxZ = max_pt.z;
00264 
00265         minX -= this->resolution_ * 0.5f;
00266         minY -= this->resolution_ * 0.5f;
00267         minZ -= this->resolution_ * 0.5f;
00268 
00269         maxX += this->resolution_ * 0.5f;
00270         maxY += this->resolution_ * 0.5f;
00271         maxZ += this->resolution_ * 0.5f;
00272 
00273         // generate bit masks for octree
00274         defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
00275 
00276       }
00277 
00279     template<typename PointT, typename LeafT, typename OctreeT>
00280       void
00281       OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox (const double minX_arg, const double minY_arg,
00282                                                                    const double minZ_arg, const double maxX_arg,
00283                                                                    const double maxY_arg, const double maxZ_arg)
00284       {
00285 
00286         // bounding box cannot be changed once the octree contains elements
00287         assert ( this->leafCount_ == 0 );
00288 
00289         assert ( maxX_arg >= minX_arg );
00290         assert ( maxY_arg >= minY_arg );
00291         assert ( maxZ_arg >= minZ_arg );
00292 
00293         minX_ = minX_arg;
00294         maxX_ = maxX_arg;
00295 
00296         minY_ = minY_arg;
00297         maxY_ = maxY_arg;
00298 
00299         minZ_ = minZ_arg;
00300         maxZ_ = maxZ_arg;
00301 
00302         minX_ = min (minX_, maxX_);
00303         minY_ = min (minY_, maxY_);
00304         minZ_ = min (minZ_, maxZ_);
00305 
00306         maxX_ = max (minX_, maxX_);
00307         maxY_ = max (minY_, maxY_);
00308         maxZ_ = max (minZ_, maxZ_);
00309 
00310         // generate bit masks for octree
00311         getKeyBitSize ();
00312 
00313         boundingBoxDefined_ = true;
00314 
00315       }
00316 
00318     template<typename PointT, typename LeafT, typename OctreeT>
00319       void
00320       OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox (const double maxX_arg, const double maxY_arg,
00321                                                                    const double maxZ_arg)
00322       {
00323 
00324         // bounding box cannot be changed once the octree contains elements
00325         assert ( this->leafCount_ == 0 );
00326 
00327         assert ( maxX_arg >= 0.0f );
00328         assert ( maxY_arg >= 0.0f );
00329         assert ( maxZ_arg >= 0.0f );
00330 
00331         minX_ = 0.0f;
00332         maxX_ = maxX_arg;
00333 
00334         minY_ = 0.0f;
00335         maxY_ = maxY_arg;
00336 
00337         minZ_ = 0.0f;
00338         maxZ_ = maxZ_arg;
00339 
00340         minX_ = min (minX_, maxX_);
00341         minY_ = min (minY_, maxY_);
00342         minZ_ = min (minZ_, maxZ_);
00343 
00344         maxX_ = max (minX_, maxX_);
00345         maxY_ = max (minY_, maxY_);
00346         maxZ_ = max (minZ_, maxZ_);
00347 
00348         // generate bit masks for octree
00349         getKeyBitSize ();
00350 
00351         boundingBoxDefined_ = true;
00352 
00353       }
00354 
00356     template<typename PointT, typename LeafT, typename OctreeT>
00357       void
00358       OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox (const double cubeLen_arg)
00359       {
00360 
00361         // bounding box cannot be changed once the octree contains elements
00362         assert ( this->leafCount_ == 0 );
00363 
00364         assert ( cubeLen_arg >= 0.0f );
00365 
00366         minX_ = 0.0f;
00367         maxX_ = cubeLen_arg;
00368 
00369         minY_ = 0.0f;
00370         maxY_ = cubeLen_arg;
00371 
00372         minZ_ = 0.0f;
00373         maxZ_ = cubeLen_arg;
00374 
00375         minX_ = min (minX_, maxX_);
00376         minY_ = min (minY_, maxY_);
00377         minZ_ = min (minZ_, maxZ_);
00378 
00379         maxX_ = max (minX_, maxX_);
00380         maxY_ = max (minY_, maxY_);
00381         maxZ_ = max (minZ_, maxZ_);
00382 
00383         // generate bit masks for octree
00384         getKeyBitSize ();
00385 
00386         boundingBoxDefined_ = true;
00387 
00388       }
00389 
00391     template<typename PointT, typename LeafT, typename OctreeT>
00392       void
00393       OctreePointCloud<PointT, LeafT, OctreeT>::getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg,
00394                                                                 double& maxX_arg, double& maxY_arg, double& maxZ_arg) const
00395       {
00396         minX_arg = minX_;
00397         minY_arg = minY_;
00398         minZ_arg = minZ_;
00399 
00400         maxX_arg = maxX_;
00401         maxY_arg = maxY_;
00402         maxZ_arg = maxZ_;
00403       }
00404 
00406     template<typename PointT, typename LeafT, typename OctreeT>
00407       void
00408       OctreePointCloud<PointT, LeafT, OctreeT>::adoptBoundingBoxToPoint (const PointT& pointIdx_arg)
00409       {
00410 
00411         const double minValue = 1e-10;
00412 
00413         // increase octree size until point fits into bounding box
00414         while (true)
00415         {
00416 
00417           bool bLowerBoundViolationX = (pointIdx_arg.x < minX_);
00418           bool bLowerBoundViolationY = (pointIdx_arg.y < minY_);
00419           bool bLowerBoundViolationZ = (pointIdx_arg.z < minZ_);
00420 
00421           bool bUpperBoundViolationX = (pointIdx_arg.x >= maxX_);
00422           bool bUpperBoundViolationY = (pointIdx_arg.y >= maxY_);
00423           bool bUpperBoundViolationZ = (pointIdx_arg.z >= maxZ_);
00424 
00425           // do we violate any bounds?
00426           if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
00427               || bUpperBoundViolationY || bUpperBoundViolationZ || (!boundingBoxDefined_))
00428           {
00429 
00430             double octreeSideLen;
00431             unsigned char childIdx;
00432 
00433             if (this->leafCount_ > 0)
00434             {
00435 
00436               // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2
00437               childIdx = ((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1) | ((!bUpperBoundViolationZ));
00438 
00439               OctreeBranch* newRootBranch;
00440 
00441               this->createBranch (newRootBranch);
00442               this->branchCount_++;
00443 
00444               this->setBranchChild (*newRootBranch, childIdx, this->rootNode_);
00445 
00446               this->rootNode_ = newRootBranch;
00447 
00448               octreeSideLen = maxX_ - minX_ - minValue;
00449 
00450               if (bUpperBoundViolationX)
00451               {
00452                 maxX_ += octreeSideLen;
00453               }
00454               else
00455               {
00456                 minX_ -= octreeSideLen;
00457               }
00458 
00459               if (bUpperBoundViolationY)
00460               {
00461                 maxY_ += octreeSideLen;
00462               }
00463               else
00464               {
00465                 minY_ -= octreeSideLen;
00466               }
00467 
00468               if (bUpperBoundViolationZ)
00469               {
00470                 maxZ_ += octreeSideLen;
00471               }
00472               else
00473               {
00474                 minZ_ -= octreeSideLen;
00475               }
00476 
00477             }
00478             else
00479             {
00480 
00481               // octree is empty - we set the center of the bounding box to our first pixel
00482               this->minX_ = pointIdx_arg.x - this->resolution_ / 2 + minValue;
00483               this->minY_ = pointIdx_arg.y - this->resolution_ / 2 + minValue;
00484               this->minZ_ = pointIdx_arg.z - this->resolution_ / 2 + minValue;
00485 
00486               this->maxX_ = pointIdx_arg.x + this->resolution_ / 2 - minValue;
00487               this->maxY_ = pointIdx_arg.y + this->resolution_ / 2 - minValue;
00488               this->maxZ_ = pointIdx_arg.z + this->resolution_ / 2 - minValue;
00489 
00490             }
00491 
00492             getKeyBitSize ();
00493 
00494             boundingBoxDefined_ = true;
00495 
00496           }
00497           else
00498           {
00499             // no bound violations anymore - leave while loop
00500             break;
00501           }
00502         }
00503 
00504       }
00505 
00507     template<typename PointT, typename LeafT, typename OctreeT>
00508       void
00509       OctreePointCloud<PointT, LeafT, OctreeT>::addPointIdx (const int pointIdx_arg)
00510       {
00511         OctreeKey key;
00512 
00513         assert (pointIdx_arg < (int) input_->points.size());
00514 
00515         const PointT& point = input_->points[pointIdx_arg];
00516 
00517         // make sure bounding box is big enough
00518         adoptBoundingBoxToPoint (point);
00519 
00520         // generate key
00521         genOctreeKeyforPoint (point, key);
00522 
00523         // add point to octree at key
00524         this->add (key, pointIdx_arg);
00525 
00526       }
00527 
00529     template<typename PointT, typename LeafT, typename OctreeT>
00530       const PointT&
00531       OctreePointCloud<PointT, LeafT, OctreeT>::getPointByIndex (const unsigned int index_arg) const
00532       {
00533         // retrieve point from input cloud
00534 
00535         if (indices_ == 0)
00536         {
00537           assert (index_arg < (unsigned int)input_->points.size ());
00538           return this->input_->points[index_arg];
00539         }
00540         else
00541         {
00542           assert (index_arg < (unsigned int)indices_->size ());
00543           return input_->points[(*indices_)[index_arg]];
00544         }
00545 
00546       }
00547 
00549     template<typename PointT, typename LeafT, typename OctreeT>
00550       LeafT*
00551       OctreePointCloud<PointT, LeafT, OctreeT>::findLeafAtPoint (const PointT& point) const
00552       {
00553         OctreeKey key;
00554 
00555         // generate key for point
00556         this->genOctreeKeyforPoint (point, key);
00557 
00558         return this->findLeaf (key);
00559       }
00560 
00562     template<typename PointT, typename LeafT, typename OctreeT>
00563       void
00564       OctreePointCloud<PointT, LeafT, OctreeT>::getKeyBitSize ()
00565       {
00566         unsigned int maxVoxels;
00567 
00568         unsigned int maxKeyX;
00569         unsigned int maxKeyY;
00570         unsigned int maxKeyZ;
00571 
00572         double octreeSideLen;
00573 
00574         const double minValue = 1e-10;
00575 
00576         // find maximum key values for x, y, z
00577         maxKeyX = ceil ((maxX_ - minX_) / resolution_);
00578         maxKeyY = ceil ((maxY_ - minY_) / resolution_);
00579         maxKeyZ = ceil ((maxZ_ - minZ_) / resolution_);
00580 
00581         // find maximum amount of keys
00582         maxVoxels = max (max (max (maxKeyX, maxKeyY), maxKeyZ), (unsigned int)2);
00583 
00584         // tree depth == amount of bits of maxVoxels
00585         this->octreeDepth_ = max ((min ((unsigned int)OCT_MAXTREEDEPTH, (unsigned int)ceil (this->Log2 (maxVoxels)))),
00586                                   (unsigned int)0);
00587 
00588         maxKeys_ = (1 << this->octreeDepth_);
00589 
00590         octreeSideLen = (double)maxKeys_ * resolution_ - minValue;
00591 
00592         if (this->leafCount_ == 0)
00593         {
00594 
00595           double octreeOversizeX;
00596           double octreeOversizeY;
00597           double octreeOversizeZ;
00598 
00599           octreeOversizeX = (octreeSideLen - (maxX_ - minX_)) / 2.0;
00600           octreeOversizeY = (octreeSideLen - (maxY_ - minY_)) / 2.0;
00601           octreeOversizeZ = (octreeSideLen - (maxZ_ - minZ_)) / 2.0;
00602 
00603           minX_ -= octreeOversizeX;
00604           minY_ -= octreeOversizeY;
00605           minZ_ -= octreeOversizeZ;
00606 
00607           maxX_ += octreeOversizeX;
00608           maxY_ += octreeOversizeY;
00609           maxZ_ += octreeOversizeZ;
00610 
00611         }
00612         else
00613         {
00614 
00615           maxX_ = minX_ + octreeSideLen;
00616           maxY_ = minY_ + octreeSideLen;
00617           maxZ_ = minZ_ + octreeSideLen;
00618 
00619         }
00620 
00621         // configure tree depth of octree
00622         this->setTreeDepth (this->octreeDepth_);
00623 
00624         return;
00625 
00626       }
00627 
00629     template<typename PointT, typename LeafT, typename OctreeT>
00630       void
00631       OctreePointCloud<PointT, LeafT, OctreeT>::genOctreeKeyforPoint (const PointT& point_arg, OctreeKey & key_arg) const
00632       {
00633 
00634         // calculate integer key for point coordinates
00635         key_arg.x = (unsigned int)((point_arg.x - this->minX_) / this->resolution_);
00636         key_arg.y = (unsigned int)((point_arg.y - this->minY_) / this->resolution_);
00637         key_arg.z = (unsigned int)((point_arg.z - this->minZ_) / this->resolution_);
00638 
00639       }
00640 
00642     template<typename PointT, typename LeafT, typename OctreeT>
00643       void
00644       OctreePointCloud<PointT, LeafT, OctreeT>::genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg,
00645                                                                       const double pointZ_arg, OctreeKey & key_arg) const
00646       {
00647         PointT tempPoint;
00648 
00649         tempPoint.x = pointX_arg;
00650         tempPoint.y = pointY_arg;
00651         tempPoint.z = pointZ_arg;
00652 
00653         // generate key for point
00654         genOctreeKeyforPoint (tempPoint, key_arg);
00655       }
00656 
00658     template<typename PointT, typename LeafT, typename OctreeT>
00659       bool
00660       OctreePointCloud<PointT, LeafT, OctreeT>::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const
00661       {
00662 
00663         const PointT tempPoint = getPointByIndex (data_arg);
00664 
00665         // generate key for point
00666         genOctreeKeyforPoint (tempPoint, key_arg);
00667 
00668         return true;
00669 
00670       }
00671 
00673     template<typename PointT, typename LeafT, typename OctreeT>
00674       void
00675       OctreePointCloud<PointT, LeafT, OctreeT>::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const
00676       {
00677 
00678         // define point to leaf node voxel center
00679         point.x = ((double)key.x + 0.5f) * this->resolution_ + this->minX_;
00680         point.y = ((double)key.y + 0.5f) * this->resolution_ + this->minY_;
00681         point.z = ((double)key.z + 0.5f) * this->resolution_ + this->minZ_;
00682 
00683       }
00684 
00686     template<typename PointT, typename LeafT, typename OctreeT>
00687       void
00688       OctreePointCloud<PointT, LeafT, OctreeT>::genVoxelCenterFromOctreeKey (const OctreeKey & key_arg,
00689                                                                              unsigned int treeDepth_arg,
00690                                                                              PointT& point_arg) const
00691       {
00692 
00693         // generate point for voxel center defined by treedepth (bitLen) and key
00694         point_arg.x = ((double)(key_arg.x) + 0.5f) * (this->resolution_ * (double)(1 << (this->octreeDepth_
00695             - treeDepth_arg))) + this->minX_;
00696         point_arg.y = ((double)(key_arg.y) + 0.5f) * (this->resolution_ * (double)(1 << (this->octreeDepth_
00697             - treeDepth_arg))) + this->minY_;
00698         point_arg.z = ((double)(key_arg.z) + 0.5f) * (this->resolution_ * (double)(1 << (this->octreeDepth_
00699             - treeDepth_arg))) + this->minZ_;
00700       }
00701 
00703     template<typename PointT, typename LeafT, typename OctreeT>
00704       void
00705       OctreePointCloud<PointT, LeafT, OctreeT>::genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg,
00706                                                                              unsigned int treeDepth_arg,
00707                                                                              Eigen::Vector3f &min_pt,
00708                                                                              Eigen::Vector3f &max_pt) const
00709       {
00710         // calculate voxel size of current tree depth
00711         double voxel_side_len = this->resolution_ * (double)(1 << (this->octreeDepth_ - treeDepth_arg));
00712 
00713         // calculate voxel bounds
00714         min_pt (0) = (double)(key_arg.x) * voxel_side_len + this->minX_;
00715         min_pt (1) = (double)(key_arg.y) * voxel_side_len + this->minY_;
00716         min_pt (2) = (double)(key_arg.z) * voxel_side_len + this->minZ_;
00717 
00718         max_pt (0) = (double)(key_arg.x + 1) * voxel_side_len + this->minX_;
00719         max_pt (1) = (double)(key_arg.y + 1) * voxel_side_len + this->minY_;
00720         max_pt (2) = (double)(key_arg.z + 1) * voxel_side_len + this->minZ_;
00721 
00722       }
00723 
00725     template<typename PointT, typename LeafT, typename OctreeT>
00726       double
00727       OctreePointCloud<PointT, LeafT, OctreeT>::getVoxelSquaredSideLen (unsigned int treeDepth_arg) const
00728       {
00729         double sideLen;
00730 
00731         // side length of the voxel cube increases exponentially with the octree depth
00732         sideLen = this->resolution_ * (double)(1 << (this->octreeDepth_ - treeDepth_arg));
00733 
00734         // squared voxel side length
00735         sideLen *= sideLen;
00736 
00737         return sideLen;
00738       }
00739 
00741     template<typename PointT, typename LeafT, typename OctreeT>
00742       double
00743       OctreePointCloud<PointT, LeafT, OctreeT>::getVoxelSquaredDiameter (unsigned int treeDepth_arg) const
00744       {
00745         // return the squared side length of the voxel cube as a function of the octree depth
00746         return getVoxelSquaredSideLen (treeDepth_arg) * 3;
00747       }
00748 
00750     template<typename PointT, typename LeafT, typename OctreeT>
00751       int
00752       OctreePointCloud<PointT, LeafT, OctreeT>::getOccupiedVoxelCentersRecursive (
00753                                                                                   const OctreeBranch* node_arg,
00754                                                                                   const OctreeKey& key_arg,
00755                                                                                   std::vector<PointT,
00756                                                                                       Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const
00757       {
00758         // child iterator
00759         unsigned char childIdx;
00760 
00761         int voxelCount = 0;
00762 
00763         // iterate over all children
00764         for (childIdx = 0; childIdx < 8; childIdx++)
00765         {
00766           if (branchHasChild (*node_arg, childIdx))
00767           {
00768             const OctreeNode * childNode;
00769             childNode = getBranchChild (*node_arg, childIdx);
00770 
00771             // generate new key for current branch voxel
00772             OctreeKey newKey;
00773             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00774             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00775             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00776 
00777             switch (childNode->getNodeType ())
00778             {
00779               case BRANCH_NODE:
00780 
00781                 // recursively proceed with indexed child branch
00782                 voxelCount += getOccupiedVoxelCentersRecursive ((OctreeBranch*)childNode, newKey, voxelCenterList_arg);
00783                 break;
00784 
00785               case LEAF_NODE:
00786                 PointT newPoint;
00787 
00788                 genLeafNodeCenterFromOctreeKey (newKey, newPoint);
00789 
00790                 voxelCenterList_arg.push_back (newPoint);
00791 
00792                 voxelCount++;
00793                 break;
00794             }
00795 
00796           }
00797 
00798         }
00799 
00800         return voxelCount;
00801       }
00802 
00803   }
00804 }
00805 
00806 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataTVector<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataTVector<int> > >;
00807 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataTVector<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeLeafDataTVector<int> > >;
00808 #define PCL_INSTANTIATE_OctreePointCloudLowMemWithLeafDataTVector(T)       template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataTVector<int> , pcl::octree::OctreeLowMemBase<int, pcl::octree::OctreeLeafDataTVector<int> > >;
00809 
00810 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataT<int> > >;
00811 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeLeafDataT<int> > >;
00812 #define PCL_INSTANTIATE_OctreePointCloudLowMemWithLeafDataT(T)       template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int> , pcl::octree::OctreeLowMemBase<int, pcl::octree::OctreeLeafDataT<int> > >;
00813 
00814 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafEmpty<int> > >;
00815 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeLeafEmpty<int> > >;
00816 #define PCL_INSTANTIATE_OctreePointCloudLowMemWithEmptyLeaf(T)       template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int> , pcl::octree::OctreeLowMemBase<int, pcl::octree::OctreeLeafEmpty<int> > >;
00817 
00818 #endif /* OCTREE_POINTCLOUD_HPP_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines