Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 #ifndef OCT_NODE_INCLUDED 00030 #define OCT_NODE_INCLUDED 00031 00032 #if defined __GNUC__ 00033 # pragma GCC system_header 00034 #endif 00035 00036 #include "allocator.h" 00037 #include "binary_node.h" 00038 #include "marching_cubes_poisson.h" 00039 00040 #define DIMENSION 3 00041 00042 namespace pcl 00043 { 00044 namespace poisson 00045 { 00046 00047 template< class NodeData , class Real=float > 00048 class OctNode 00049 { 00050 private: 00051 static int UseAlloc; 00052 00053 class AdjacencyCountFunction 00054 { 00055 public: 00056 int count; 00057 void Function( const OctNode<NodeData,Real>* node1 , const OctNode<NodeData,Real>* node2 ); 00058 }; 00059 template<class NodeAdjacencyFunction> 00060 void __processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4); 00061 template<class NodeAdjacencyFunction> 00062 void __processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2); 00063 template<class NodeAdjacencyFunction> 00064 void __processNodeNodes(OctNode* node,NodeAdjacencyFunction* F); 00065 template<class NodeAdjacencyFunction> 00066 static void __ProcessNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,NodeAdjacencyFunction* F); 00067 template<class TerminatingNodeAdjacencyFunction> 00068 static void __ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,TerminatingNodeAdjacencyFunction* F); 00069 template<class PointAdjacencyFunction> 00070 static void __ProcessPointAdjacentNodes(int dx,int dy,int dz,OctNode* node2,int radius2,int cWidth2,PointAdjacencyFunction* F); 00071 template<class NodeAdjacencyFunction> 00072 static void __ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,int depth,NodeAdjacencyFunction* F); 00073 template<class NodeAdjacencyFunction> 00074 static void __ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,int depth,NodeAdjacencyFunction* F); 00075 00076 // This is made private because the division by two has been pulled out. 00077 static inline int Overlap(int c1,int c2,int c3,int dWidth); 00078 inline static int ChildOverlap(int dx,int dy,int dz,int d,int cRadius2); 00079 00080 const OctNode* __faceNeighbor(int dir,int off) const; 00081 const OctNode* __edgeNeighbor(int o,const int i[2],const int idx[2]) const; 00082 OctNode* __faceNeighbor(int dir,int off,int forceChildren); 00083 OctNode* __edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren); 00084 public: 00085 static const int DepthShift,OffsetShift,OffsetShift1,OffsetShift2,OffsetShift3; 00086 static const int DepthMask,OffsetMask; 00087 00088 static Allocator<OctNode> internalAllocator; 00089 static int UseAllocator(void); 00090 static void SetAllocator(int blockSize); 00091 00092 OctNode* parent; 00093 OctNode* children; 00094 short d , off[DIMENSION]; 00095 NodeData nodeData; 00096 00097 OctNode(void); 00098 ~OctNode(void); 00099 int initChildren(void); 00100 00101 void depthAndOffset(int& depth,int offset[DIMENSION]) const; 00102 int depth(void) const; 00103 static inline void DepthAndOffset(const long long& index,int& depth,int offset[DIMENSION]); 00104 static inline void CenterAndWidth(const long long& index,Point3D<Real>& center,Real& width); 00105 static inline int Depth(const long long& index); 00106 static inline void Index(int depth,const int offset[3],short& d,short off[DIMENSION]); 00107 void centerAndWidth( Point3D<Real>& center , Real& width ) const; 00108 bool isInside( Point3D< Real > p ) const; 00109 00110 int leaves(void) const; 00111 int maxDepthLeaves(int maxDepth) const; 00112 int nodes(void) const; 00113 int maxDepth(void) const; 00114 00115 const OctNode* root(void) const; 00116 00117 const OctNode* nextLeaf(const OctNode* currentLeaf=NULL) const; 00118 OctNode* nextLeaf(OctNode* currentLeaf=NULL); 00119 const OctNode* nextNode(const OctNode* currentNode=NULL) const; 00120 OctNode* nextNode(OctNode* currentNode=NULL); 00121 const OctNode* nextBranch(const OctNode* current) const; 00122 OctNode* nextBranch(OctNode* current); 00123 const OctNode* prevBranch(const OctNode* current) const; 00124 OctNode* prevBranch(OctNode* current); 00125 00126 void setFullDepth(int maxDepth); 00127 00128 void printLeaves(void) const; 00129 void printRange(void) const; 00130 00131 template<class NodeAdjacencyFunction> 00132 void processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent=1); 00133 template<class NodeAdjacencyFunction> 00134 void processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent=1); 00135 template<class NodeAdjacencyFunction> 00136 void processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent=1); 00137 template<class NodeAdjacencyFunction> 00138 void processNodeNodes(OctNode* node,NodeAdjacencyFunction* F,int processCurrent=1); 00139 00140 template<class NodeAdjacencyFunction> 00141 static void ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent=1); 00142 template<class NodeAdjacencyFunction> 00143 static void ProcessNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,NodeAdjacencyFunction* F,int processCurrent=1); 00144 template<class TerminatingNodeAdjacencyFunction> 00145 static void ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent=1); 00146 template<class TerminatingNodeAdjacencyFunction> 00147 static void ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent=1); 00148 template<class PointAdjacencyFunction> 00149 static void ProcessPointAdjacentNodes(int maxDepth,const int center1[3],OctNode* node2,int width2,PointAdjacencyFunction* F,int processCurrent=1); 00150 template<class PointAdjacencyFunction> 00151 static void ProcessPointAdjacentNodes(int dx,int dy,int dz,OctNode* node2,int radius2,int width2,PointAdjacencyFunction* F,int processCurrent=1); 00152 template<class NodeAdjacencyFunction> 00153 static void ProcessFixedDepthNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); 00154 template<class NodeAdjacencyFunction> 00155 static void ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); 00156 template<class NodeAdjacencyFunction> 00157 static void ProcessMaxDepthNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); 00158 template<class NodeAdjacencyFunction> 00159 static void ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); 00160 00161 static int CornerIndex(const Point3D<Real>& center,const Point3D<Real> &p); 00162 00163 OctNode* faceNeighbor(int faceIndex,int forceChildren=0); 00164 const OctNode* faceNeighbor(int faceIndex) const; 00165 OctNode* edgeNeighbor(int edgeIndex,int forceChildren=0); 00166 const OctNode* edgeNeighbor(int edgeIndex) const; 00167 OctNode* cornerNeighbor(int cornerIndex,int forceChildren=0); 00168 const OctNode* cornerNeighbor(int cornerIndex) const; 00169 00170 OctNode* getNearestLeaf(const Point3D<Real>& p); 00171 const OctNode* getNearestLeaf(const Point3D<Real>& p) const; 00172 00173 static int CommonEdge(const OctNode* node1,int eIndex1,const OctNode* node2,int eIndex2); 00174 static int CompareForwardDepths(const void* v1,const void* v2); 00175 static int CompareByDepthAndXYZ( const void* v1 , const void* v2 ); 00176 static int CompareByDepthAndZIndex( const void* v1 , const void* v2 ); 00177 static int CompareForwardPointerDepths(const void* v1,const void* v2); 00178 static int CompareBackwardDepths(const void* v1,const void* v2); 00179 static int CompareBackwardPointerDepths(const void* v1,const void* v2); 00180 00181 00182 template<class NodeData2> 00183 OctNode& operator = (const OctNode<NodeData2,Real>& node); 00184 00185 static inline int Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2); 00186 00187 00188 int write(const char* fileName) const; 00189 int write(FILE* fp) const; 00190 int read(const char* fileName); 00191 int read(FILE* fp); 00192 00193 class Neighbors3 00194 { 00195 public: 00196 OctNode* neighbors[3][3][3]; 00197 Neighbors3( void ); 00198 void clear( void ); 00199 }; 00200 class NeighborKey3 00201 { 00202 public: 00203 Neighbors3* neighbors; 00204 00205 NeighborKey3( void ); 00206 ~NeighborKey3( void ); 00207 00208 void set( int depth ); 00209 Neighbors3& setNeighbors( OctNode* root , Point3D< Real > p , int d ); 00210 Neighbors3& getNeighbors( OctNode* root , Point3D< Real > p , int d ); 00211 Neighbors3& setNeighbors( OctNode* node , bool flags[3][3][3] ); 00212 Neighbors3& setNeighbors( OctNode* node ); 00213 Neighbors3& getNeighbors( OctNode* node ); 00214 }; 00215 class ConstNeighbors3 00216 { 00217 public: 00218 const OctNode* neighbors[3][3][3]; 00219 ConstNeighbors3( void ); 00220 void clear( void ); 00221 }; 00222 class ConstNeighborKey3 00223 { 00224 public: 00225 ConstNeighbors3* neighbors; 00226 00227 ConstNeighborKey3(void); 00228 ~ConstNeighborKey3(void); 00229 00230 void set(int depth); 00231 ConstNeighbors3& getNeighbors( const OctNode* node ); 00232 ConstNeighbors3& getNeighbors( const OctNode* node , int minDepth ); 00233 }; 00234 class Neighbors5 00235 { 00236 public: 00237 OctNode* neighbors[5][5][5]; 00238 Neighbors5( void ); 00239 void clear( void ); 00240 }; 00241 class ConstNeighbors5 00242 { 00243 public: 00244 const OctNode* neighbors[5][5][5]; 00245 ConstNeighbors5( void ); 00246 void clear( void ); 00247 }; 00248 00249 class NeighborKey5 00250 { 00251 int _depth; 00252 public: 00253 Neighbors5* neighbors; 00254 00255 NeighborKey5( void ); 00256 ~NeighborKey5( void ); 00257 00258 void set( int depth ); 00259 Neighbors5& getNeighbors( OctNode* node ); 00260 Neighbors5& setNeighbors( OctNode* node , int xStart=0 , int xEnd=5 , int yStart=0 , int yEnd=5 , int zStart=0 , int zEnd=5 ); 00261 }; 00262 class ConstNeighborKey5 00263 { 00264 int _depth; 00265 public: 00266 ConstNeighbors5* neighbors; 00267 00268 ConstNeighborKey5( void ); 00269 ~ConstNeighborKey5( void ); 00270 00271 void set( int depth ); 00272 ConstNeighbors5& getNeighbors( const OctNode* node ); 00273 }; 00274 00275 void centerIndex(int maxDepth,int index[DIMENSION]) const; 00276 int width(int maxDepth) const; 00277 }; 00278 00279 00280 } 00281 } 00282 00283 #include "octree_poisson.hpp" 00284 00285 00286 00287 #endif // OCT_NODE