octree_pointcloud.h

Go to the documentation of this file.
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_POINTCLOUD_H
00038 #define OCTREE_POINTCLOUD_H
00039 
00040 #include "octree_base.h"
00041 #include "octree2buf_base.h"
00042 #include "octree_lowmemory_base.h"
00043 
00044 #include <pcl/point_cloud.h>
00045 #include <pcl/point_types.h>
00046 
00047 #include "octree_nodes.h"
00048 
00049 #include <queue>
00050 #include <vector>
00051 #include <algorithm>
00052 #include <iostream>
00053 
00054 namespace pcl
00055 {
00056   namespace octree
00057   {
00058 
00060 
00072 
00073     template<typename PointT, typename LeafT = OctreeLeafDataTVector<int> , typename OctreeT = OctreeBase<int, LeafT> >
00074       class OctreePointCloud : public OctreeT
00075       {
00076 
00077       public:
00078 
00082         OctreePointCloud (const double resolution_arg);
00083 
00085         virtual
00086         ~OctreePointCloud ();
00087 
00088         // public typedefs
00089         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00090         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00091 
00092         typedef pcl::PointCloud<PointT> PointCloud;
00093         typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00094         typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00095 
00096         // public typedefs for single/double buffering
00097         typedef OctreePointCloud<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer;
00098         typedef OctreePointCloud<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer;
00099         typedef OctreePointCloud<PointT, LeafT, OctreeLowMemBase<int, LeafT> > LowMem;
00100 
00101 
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 
00111         inline void
00112         setInputCloud (const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg = IndicesConstPtr ())
00113         {
00114           assert (this->leafCount_==0);
00115 
00116           if ((input_ != cloud_arg) && (this->leafCount_ == 0))
00117           {
00118             input_ = cloud_arg;
00119             indices_ = indices_arg;
00120           }
00121         }
00122 
00126         inline IndicesConstPtr const
00127         getIndices ()
00128         {
00129           return (indices_);
00130         }
00131 
00135         inline PointCloudConstPtr
00136         getInputCloud ()
00137         {
00138           return (input_);
00139         }
00140 
00144         inline void
00145         setEpsilon (double eps)
00146         {
00147           epsilon_ = eps;
00148         }
00149 
00151         inline double
00152         getEpsilon ()
00153         {
00154           return (epsilon_);
00155         }
00156 
00160         inline void
00161         setResolution (double resolution_arg)
00162         {
00163           // octree needs to be empty to change its resolution
00164           assert( this->leafCount_ == 0 );
00165 
00166           resolution_ = resolution_arg;
00167         }
00168 
00171         inline double
00172         getResolution ()
00173         {
00174           return (resolution_);
00175         }
00176 
00178         void
00179         addPointsFromInputCloud ();
00180 
00185         void
00186         addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg);
00187 
00192         void
00193         addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg);
00194 
00200         void
00201         addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg);
00202 
00207         bool
00208         isVoxelOccupiedAtPoint (const PointT& point_arg) const;
00209 
00211         void
00212         deleteTree()
00213         {
00214           // reset bounding box
00215           minX_ = minY_ = maxY_ = minZ_ = maxZ_ = 0;
00216           maxKeys_ = 1;
00217           this->boundingBoxDefined_ = false;
00218 
00219           OctreeT::deleteTree();
00220         }
00221 
00228         bool
00229         isVoxelOccupiedAtPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg) const;
00230 
00235         bool
00236         isVoxelOccupiedAtPoint (const int& pointIdx_arg) const;
00237 
00243         bool
00244         voxelSearch (const PointT& point_arg, std::vector<int>& pointIdx_data_arg);
00245 
00251         bool
00252         voxelSearch (const int index_arg, std::vector<int>& pointIdx_data_arg);
00253 
00263         int
00264         nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
00265                         std::vector<float> &k_sqr_distances_arg);
00266 
00274         int
00275         nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
00276                         std::vector<float> &k_sqr_distances_arg);
00277 
00287         int
00288         nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
00289                         std::vector<float> &k_sqr_distances_arg);
00290 
00298         void
00299         approxNearestSearch (const PointCloudConstPtr &cloud_arg, int query_index_arg, int &result_index_arg,
00300                              float &sqr_distance_arg);
00301 
00307         void
00308         approxNearestSearch (const PointT &p_q_arg, int &result_index_arg, float &sqr_distance_arg);
00309 
00317         void
00318         approxNearestSearch (int query_index_arg, int &result_index_arg, float &sqr_distance_arg);
00319 
00329         int
00330         radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
00331                       std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
00332                       int max_nn_arg = INT_MAX);
00333 
00342         int
00343         radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
00344                       std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
00345 
00355         int
00356         radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
00357                       std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
00358 
00363         int
00364         getOccupiedVoxelCenters (std::vector<PointT, Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const;
00365 
00369         void
00370         deleteVoxelAtPoint (const PointT& point_arg);
00371 
00375         void
00376         deleteVoxelAtPoint (const int& pointIdx_arg);
00377 
00378 
00380         // Bounding box methods
00382 
00383 
00386         void
00387         defineBoundingBox ();
00388 
00398         void
00399         defineBoundingBox (const double minX_arg, const double minY_arg, const double minZ_arg, const double maxX_arg,
00400                            const double maxY_arg, const double maxZ_arg);
00401 
00409         void
00410         defineBoundingBox (const double maxX_arg, const double maxY_arg, const double maxZ_arg);
00411 
00417         void
00418         defineBoundingBox (const double cubeLen_arg);
00419 
00429         void
00430         getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg, double& maxX_arg, double& maxY_arg,
00431                         double& maxZ_arg) const ;
00432 
00437         double
00438         getVoxelSquaredDiameter (unsigned int treeDepth_arg) const ;
00439 
00443         inline double
00444         getVoxelSquaredDiameter ( ) const
00445         {
00446           return getVoxelSquaredDiameter( this->octreeDepth_ );
00447         }
00448 
00453         double
00454         getVoxelSquaredSideLen (unsigned int treeDepth_arg) const ;
00455 
00459         inline double
00460         getVoxelSquaredSideLen () const
00461         {
00462           return getVoxelSquaredSideLen( this->octreeDepth_ );
00463         }
00464 
00465         typedef typename OctreeT::OctreeLeaf OctreeLeaf;
00466 
00467       protected:
00468 
00472         void
00473         addPointIdx (const int pointIdx_arg);
00474 
00479         const PointT&
00480         getPointByIndex (const unsigned int index_arg) const;
00481 
00486         LeafT*
00487         findLeafAtPoint (const PointT& point_arg) const ;
00488 
00490         // Protected octree methods based on octree keys
00492 
00493         typedef typename OctreeT::OctreeKey OctreeKey;
00494         typedef typename OctreeT::OctreeBranch OctreeBranch;
00495 
00498         void
00499         getKeyBitSize ();
00500 
00504         void
00505         adoptBoundingBoxToPoint (const PointT& pointIdx_arg);
00506 
00511         void
00512         genOctreeKeyforPoint (const PointT & point_arg, OctreeKey & key_arg) const ;
00513 
00520         void
00521         genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg,
00522                               OctreeKey & key_arg) const;
00523 
00530         virtual bool
00531         genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const;
00532 
00537         void
00538         genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg, PointT& point_arg) const ;
00539 
00545         void
00546         genVoxelCenterFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, PointT& point_arg) const ;
00547 
00549         // Octree-based search routines & helpers
00551 
00552 
00554 
00558 
00559         class prioBranchQueueEntry
00560         {
00561         public:
00562 
00564           prioBranchQueueEntry ()
00565           {
00566           }
00567 
00573           prioBranchQueueEntry (OctreeNode* node_arg, OctreeKey& key_arg, double pointDistance_arg)
00574           {
00575             node = node_arg;
00576             pointDistance = pointDistance_arg;
00577             key = key_arg;
00578           }
00579 
00581           bool
00582           operator< (const prioBranchQueueEntry rhs_arg) const
00583           {
00584             return (this->pointDistance > rhs_arg.pointDistance);
00585           }
00586 
00587           // pointer to octree node
00588           const OctreeNode* node;
00589 
00590           // distance to query point
00591           double pointDistance;
00592 
00593           // octree key
00594           OctreeKey key;
00595 
00596         };
00597 
00599 
00603 
00604         class prioPointQueueEntry
00605         {
00606         public:
00607 
00609           prioPointQueueEntry ()
00610           {
00611           }
00612 
00617           prioPointQueueEntry (unsigned int& pointIdx_arg, double pointDistance_arg)
00618           {
00619             pointIdx_ = pointIdx_arg;
00620             pointDistance_ = pointDistance_arg;
00621           }
00622 
00624           bool
00625           operator< (const prioPointQueueEntry& rhs_arg) const
00626           {
00627             return (this->pointDistance_ < rhs_arg.pointDistance_);
00628           }
00629 
00630           // index representing a point in the dataset given by \a setInputCloud
00631           int pointIdx_;
00632 
00633           // distance to query point
00634           double pointDistance_;
00635 
00636         };
00637 
00643         double
00644         pointSquaredDist (const PointT & pointA_arg, const PointT & pointB_arg) const;
00645 
00647         // Recursive search routine methods
00649 
00650 
00661         void
00662         getNeighborsWithinRadiusRecursive (const PointT & point_arg, const double radiusSquared_arg,
00663                                            const OctreeBranch* node_arg, const OctreeKey& key_arg,
00664                                            unsigned int treeDepth_arg, std::vector<int>& k_indices_arg,
00665                                            std::vector<float>& k_sqr_distances_arg, int max_nn_arg) const;
00666 
00677         double
00678         getKNearestNeighborRecursive (const PointT & point_arg, unsigned int K_arg, const OctreeBranch* node_arg,
00679                                       const OctreeKey& key_arg, unsigned int treeDepth_arg,
00680                                       const double squaredSearchRadius_arg,
00681                                       std::vector<prioPointQueueEntry>& pointCandidates_arg) const;
00682 
00691         void
00692         approxNearestSearchRecursive (const PointT & point_arg, const OctreeBranch* node_arg, const OctreeKey& key_arg,
00693                                       unsigned int treeDepth_arg, int& result_index_arg,
00694                                       float& sqr_distance_arg);
00695 
00702         int
00703         getOccupiedVoxelCentersRecursive (const OctreeBranch* node_arg, const OctreeKey& key_arg,
00704                                           std::vector<PointT, Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const;
00705 
00706 
00707 
00709         // Globals
00711 
00713         PointCloudConstPtr input_;
00714 
00716         IndicesConstPtr indices_;
00717 
00719         double epsilon_;
00720 
00722         double resolution_;
00723 
00724         // Octree bounding box coordinates
00725         double minX_;
00726         double maxX_;
00727 
00728         double minY_;
00729         double maxY_;
00730 
00731         double minZ_;
00732         double maxZ_;
00733 
00735         unsigned int maxKeys_;
00736 
00738         bool boundingBoxDefined_;
00739 
00740       };
00741 
00742   }
00743 }
00744 
00745 #endif
00746