Point Cloud Library (PCL)  1.3.1
octree_pointcloud.h
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.h 3017 2011-11-01 03:24:04Z rusu $
00037  */
00038 
00039 #ifndef OCTREE_POINTCLOUD_H
00040 #define OCTREE_POINTCLOUD_H
00041 
00042 #include "octree_base.h"
00043 #include "octree2buf_base.h"
00044 #include "octree_lowmemory_base.h"
00045 
00046 #include <pcl/point_cloud.h>
00047 #include <pcl/point_types.h>
00048 
00049 #include "octree_nodes.h"
00050 
00051 #include <queue>
00052 #include <vector>
00053 #include <algorithm>
00054 #include <iostream>
00055 
00056 namespace pcl
00057 {
00058   namespace octree
00059   {
00060 
00062 
00074 
00075     template<typename PointT, typename LeafT = OctreeLeafDataTVector<int> , typename OctreeT = OctreeBase<int, LeafT> >
00076       class OctreePointCloud : public OctreeT
00077       {
00078 
00079       public:
00080 
00084         OctreePointCloud (const double resolution_arg);
00085 
00087         virtual
00088         ~OctreePointCloud ();
00089 
00090         // public typedefs
00091         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00092         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00093 
00094         typedef pcl::PointCloud<PointT> PointCloud;
00095         typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00096         typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00097 
00098         // public typedefs for single/double buffering
00099         typedef OctreePointCloud<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer;
00100         typedef OctreePointCloud<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer;
00101         typedef OctreePointCloud<PointT, LeafT, OctreeLowMemBase<int, LeafT> > LowMem;
00102 
00103         // Boost shared pointers
00104         typedef boost::shared_ptr<OctreePointCloud<PointT, LeafT, OctreeT> > Ptr;
00105         typedef boost::shared_ptr<const OctreePointCloud<PointT, LeafT, OctreeT> > ConstPtr;
00106 
00107         // Eigen aligned allocator
00108         typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
00109 
00114         inline void
00115         setInputCloud (const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg = IndicesConstPtr ())
00116         {
00117           assert (this->leafCount_==0);
00118 
00119           if ((input_ != cloud_arg) && (this->leafCount_ == 0))
00120           {
00121             input_ = cloud_arg;
00122             indices_ = indices_arg;
00123           }
00124         }
00125 
00129         inline IndicesConstPtr const
00130         getIndices ()
00131         {
00132           return (indices_);
00133         }
00134 
00138         inline PointCloudConstPtr
00139         getInputCloud ()
00140         {
00141           return (input_);
00142         }
00143 
00147         inline void
00148         setEpsilon (double eps)
00149         {
00150           epsilon_ = eps;
00151         }
00152 
00154         inline double
00155         getEpsilon ()
00156         {
00157           return (epsilon_);
00158         }
00159 
00163         inline void
00164         setResolution (double resolution_arg)
00165         {
00166           // octree needs to be empty to change its resolution
00167           assert( this->leafCount_ == 0 );
00168 
00169           resolution_ = resolution_arg;
00170         }
00171 
00174         inline double
00175         getResolution ()
00176         {
00177           return (resolution_);
00178         }
00179 
00181         void
00182         addPointsFromInputCloud ();
00183 
00188         void
00189         addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg);
00190 
00195         void
00196         addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg);
00197 
00203         void
00204         addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg);
00205 
00210         bool
00211         isVoxelOccupiedAtPoint (const PointT& point_arg) const;
00212 
00214         void
00215         deleteTree ()
00216         {
00217           // reset bounding box
00218           minX_ = minY_ = maxY_ = minZ_ = maxZ_ = 0;
00219           maxKeys_ = 1;
00220           this->boundingBoxDefined_ = false;
00221 
00222           OctreeT::deleteTree ();
00223         }
00224 
00231         bool
00232         isVoxelOccupiedAtPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg) const;
00233 
00238         bool
00239         isVoxelOccupiedAtPoint (const int& pointIdx_arg) const;
00240 
00245         int
00246         getOccupiedVoxelCenters (AlignedPointTVector &voxelCenterList_arg) const;
00247 
00251         void
00252         deleteVoxelAtPoint (const PointT& point_arg);
00253 
00257         void
00258         deleteVoxelAtPoint (const int& pointIdx_arg);
00259 
00261         // Bounding box methods
00263 
00264 
00267         void
00268         defineBoundingBox ();
00269 
00279         void
00280         defineBoundingBox (const double minX_arg, const double minY_arg, const double minZ_arg, const double maxX_arg,
00281                            const double maxY_arg, const double maxZ_arg);
00282 
00290         void
00291         defineBoundingBox (const double maxX_arg, const double maxY_arg, const double maxZ_arg);
00292 
00298         void
00299         defineBoundingBox (const double cubeLen_arg);
00300 
00310         void
00311         getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg, double& maxX_arg, double& maxY_arg,
00312                         double& maxZ_arg) const ;
00313 
00318         double
00319         getVoxelSquaredDiameter (unsigned int treeDepth_arg) const ;
00320 
00324         inline double
00325         getVoxelSquaredDiameter () const
00326         {
00327           return getVoxelSquaredDiameter (this->octreeDepth_);
00328         }
00329 
00334         double
00335         getVoxelSquaredSideLen (unsigned int treeDepth_arg) const ;
00336 
00340         inline double
00341         getVoxelSquaredSideLen () const
00342         {
00343           return getVoxelSquaredSideLen (this->octreeDepth_);
00344         }
00345 
00346         typedef typename OctreeT::OctreeLeaf OctreeLeaf;
00347 
00348       protected:
00349 
00353         void
00354         addPointIdx (const int pointIdx_arg);
00355 
00360         const PointT&
00361         getPointByIndex (const unsigned int index_arg) const;
00362 
00367         LeafT*
00368         findLeafAtPoint (const PointT& point_arg) const ;
00369 
00371         // Protected octree methods based on octree keys
00373 
00374         typedef typename OctreeT::OctreeKey OctreeKey;
00375         typedef typename OctreeT::OctreeBranch OctreeBranch;
00376 
00379         void
00380         getKeyBitSize ();
00381 
00385         void
00386         adoptBoundingBoxToPoint (const PointT& pointIdx_arg);
00387 
00392         void
00393         genOctreeKeyforPoint (const PointT & point_arg, OctreeKey & key_arg) const ;
00394 
00401         void
00402         genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg,
00403                               OctreeKey & key_arg) const;
00404 
00411         virtual bool
00412         genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const;
00413 
00418         void
00419         genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg, PointT& point_arg) const ;
00420 
00426         void
00427         genVoxelCenterFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, PointT& point_arg) const ;
00428 
00435         void
00436         genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, Eigen::Vector3f &min_pt,
00437                         Eigen::Vector3f &max_pt) const;
00438 
00445         int
00446         getOccupiedVoxelCentersRecursive (const OctreeBranch* node_arg,
00447                                           const OctreeKey& key_arg,
00448                                           std::vector<PointT, Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const;
00449 
00451         // Globals
00453 
00455         PointCloudConstPtr input_;
00456 
00458         IndicesConstPtr indices_;
00459 
00461         double epsilon_;
00462 
00464         double resolution_;
00465 
00466         // Octree bounding box coordinates
00467         double minX_;
00468         double maxX_;
00469 
00470         double minY_;
00471         double maxY_;
00472 
00473         double minZ_;
00474         double maxZ_;
00475 
00477         unsigned int maxKeys_;
00478 
00480         bool boundingBoxDefined_;
00481 
00482       };
00483 
00484   }
00485 }
00486 
00487 #endif
00488 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines