Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012, Jeremie Papon 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 * Author : jpapon@gmail.com 00037 * Email : jpapon@gmail.com 00038 */ 00039 00040 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_H_ 00041 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_H_ 00042 00043 #include <pcl/common/geometry.h> 00044 #include <pcl/octree/boost.h> 00045 #include <pcl/octree/octree_base.h> 00046 #include <pcl/octree/octree_iterator.h> 00047 #include <pcl/octree/octree_pointcloud.h> 00048 #include <pcl/octree/octree_pointcloud_adjacency_container.h> 00049 00050 #include <set> 00051 #include <list> 00052 00053 //DEBUG TODO REMOVE 00054 #include <pcl/common/time.h> 00055 00056 00057 00058 namespace pcl 00059 { 00060 00061 namespace octree 00062 { 00063 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00064 /** \brief @b Octree pointcloud voxel class used for adjacency calculation 00065 * \note This pointcloud octree class generate an octree from a point cloud (zero-copy). 00066 * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. 00067 * \note This class maintains adjacency information for its voxels 00068 * \note The OctreePointCloudAdjacencyContainer can be used to store data in leaf nodes 00069 * \note An optional transform function can be provided which changes how the voxel grid is computed - this can be used to, for example, make voxel bins larger as they increase in distance from the origin (camera) 00070 * \note See SupervoxelClustering for an example of how to provide a transform function 00071 * \note If used in academic work, please cite: 00072 * 00073 * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter 00074 * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds 00075 * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 00076 * \ingroup octree 00077 * \author Jeremie Papon (jpapon@gmail.com) 00078 */ 00079 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00080 template< typename PointT, 00081 typename LeafContainerT = OctreePointCloudAdjacencyContainer <PointT>, 00082 typename BranchContainerT = OctreeContainerEmpty > 00083 class OctreePointCloudAdjacency : public OctreePointCloud< PointT, LeafContainerT, BranchContainerT> 00084 00085 { 00086 00087 public: 00088 typedef OctreeBase<LeafContainerT, BranchContainerT> OctreeBaseT; 00089 00090 typedef OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT; 00091 typedef boost::shared_ptr<OctreeAdjacencyT> Ptr; 00092 typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr; 00093 00094 typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT,OctreeBaseT > OctreePointCloudT; 00095 typedef typename OctreePointCloudT::LeafNode LeafNode; 00096 typedef typename OctreePointCloudT::BranchNode BranchNode; 00097 00098 typedef pcl::PointCloud<PointT> CloudT; 00099 00100 //So we can access input 00101 using OctreePointCloudT::input_; 00102 using OctreePointCloudT::resolution_; 00103 00104 // iterators are friends 00105 friend class OctreeIteratorBase<OctreeAdjacencyT> ; 00106 friend class OctreeDepthFirstIterator<OctreeAdjacencyT> ; 00107 friend class OctreeBreadthFirstIterator<OctreeAdjacencyT> ; 00108 friend class OctreeLeafNodeIterator<OctreeAdjacencyT> ; 00109 00110 // Octree default iterators 00111 typedef OctreeDepthFirstIterator<OctreeAdjacencyT> Iterator; 00112 typedef const OctreeDepthFirstIterator<OctreeAdjacencyT> ConstIterator; 00113 Iterator depth_begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);}; 00114 const Iterator depth_end() {return Iterator();}; 00115 00116 // Octree leaf node iterators 00117 typedef OctreeLeafNodeIterator<OctreeAdjacencyT> LeafNodeIterator; 00118 typedef const OctreeLeafNodeIterator< OctreeAdjacencyT> ConstLeafNodeIterator; 00119 LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);}; 00120 const LeafNodeIterator leaf_end() {return LeafNodeIterator();}; 00121 00122 typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList; 00123 typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID; 00124 typedef typename VoxelAdjacencyList::edge_descriptor EdgeID; 00125 00126 //Leaf vector - pointers to all leaves 00127 typedef std::vector <LeafContainerT*> LeafVectorT; 00128 //Fast leaf iterators that don't require traversing tree 00129 typedef typename LeafVectorT::iterator iterator; 00130 typedef typename LeafVectorT::const_iterator const_iterator; 00131 inline iterator begin () { return (leaf_vector_.begin ()); } 00132 inline iterator end () { return (leaf_vector_.end ()); } 00133 //size of neighbors 00134 inline size_t size () const { return leaf_vector_.size (); } 00135 00136 00137 /** \brief Constructor. 00138 * \param[in] resolution_arg octree resolution at lowest octree level (voxel size) 00139 **/ 00140 OctreePointCloudAdjacency (const double resolution_arg); 00141 00142 00143 /** \brief Empty class destructor. */ 00144 virtual ~OctreePointCloudAdjacency () 00145 { 00146 } 00147 00148 /** \brief Adds points from cloud to the octree 00149 * \note This overrides the addPointsFromInputCloud from the OctreePointCloud class 00150 */ 00151 void 00152 addPointsFromInputCloud (); 00153 00154 /** \brief Gets the leaf container for a given point 00155 * \param[in] point_arg Point to search for 00156 * \returns Pointer to the leaf container - null if no leaf container found 00157 */ 00158 LeafContainerT * 00159 getLeafContainerAtPoint (const PointT& point_arg) const; 00160 00161 /** \brief Computes an adjacency graph of voxel relations 00162 * \note WARNING: This slows down rapidly as cloud size increases due number of edges 00163 * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships - vertices are pointT, edges represent touching, and edge lengths are the distance between the points 00164 */ 00165 void 00166 computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph); 00167 00168 /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud 00169 * This is useful for changing how adjacency is calculated - such as relaxing 00170 * the adjacency criterion for points further from the camera 00171 * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one parameter (a point) which it modifies in place 00172 */ 00173 void 00174 setTransformFunction (boost::function<void (PointT &p)> transform_func) 00175 { 00176 transform_func_ = transform_func; 00177 } 00178 00179 /** \brief Tests whether input point is occluded from specified camera point by other voxels 00180 \param[in] point_arg Point to test for 00181 \param[in] camera_pos Position of camera, defaults to origin 00182 \returns True if path to camera is blocked by a voxel, false otherwise 00183 */ 00184 bool 00185 testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0,0,0)); 00186 00187 protected: 00188 00189 /** \brief Add point at index from input pointcloud dataset to octree 00190 * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added 00191 * \note This virtual implementation allows the use of a transform function to compute keys 00192 */ 00193 virtual void 00194 addPointIdx (const int pointIdx_arg); 00195 00196 /** \brief Fills in the neighbors fields for new voxels 00197 * \param[in] key_arg Key of the voxel to check neighbors for 00198 * \param[in] leaf_container Pointer to container of the leaf to check neighbors for 00199 */ 00200 void 00201 computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container); 00202 00203 /** \brief Generates octree key for specified point (uses transform if provided) 00204 * \param[in] point_arg Point to generate key for 00205 * \param[out] key_arg Resulting octree key 00206 */ 00207 void 00208 genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const; 00209 00210 private: 00211 00212 StopWatch timer_; 00213 00214 //Local leaf pointer vector used to make iterating through leaves fast 00215 LeafVectorT leaf_vector_; 00216 00217 boost::function<void (PointT &p)> transform_func_; 00218 }; 00219 00220 } 00221 00222 } 00223 00224 00225 00226 00227 00228 00229 00230 00231 00232 //#ifdef PCL_NO_PRECOMPILE 00233 #include <pcl/octree/impl/octree_pointcloud_adjacency.hpp> 00234 //#endif 00235 00236 #endif //PCL_OCTREE_POINTCLOUD_SUPER_VOXEL_H_ 00237