Point Cloud Library (PCL)  1.9.1-dev
multi_grid_octree_data.h
1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12 
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
27 */
28 
29 #ifndef MULTI_GRID_OCTREE_DATA_INCLUDED
30 #define MULTI_GRID_OCTREE_DATA_INCLUDED
31 
32 #if defined __GNUC__
33 # pragma GCC system_header
34 #endif
35 
36 #define MISHA_DEBUG 1
37 
38 #define GRADIENT_DOMAIN_SOLUTION 1 // Given the constraint vector-field V(p), there are two ways to solve for the coefficients, x, of the indicator function
39 // with respect to the B-spline basis {B_i(p)}
40 // 1] Find x minimizing:
41 // || V(p) - \sum_i \nabla x_i B_i(p) ||^2
42 // which is solved by the system A_1x = b_1 where:
43 // A_1[i,j] = < \nabla B_i(p) , \nabla B_j(p) >
44 // b_1[i] = < \nabla B_i(p) , V(p) >
45 // 2] Formulate this as a Poisson equation:
46 // \sum_i x_i \Delta B_i(p) = \nabla \cdot V(p)
47 // which is solved by the system A_2x = b_2 where:
48 // A_2[i,j] = - < \Delta B_i(p) , B_j(p) >
49 // b_2[i] = - < B_i(p) , \nabla \cdot V(p) >
50 // Although the two system matrices should be the same (assuming that the B_i satisfy dirichlet/neumann boundary conditions)
51 // the constraint vectors can differ when V does not satisfy the Neumann boundary conditions:
52 // A_1[i,j] = \int_R < \nabla B_i(p) , \nabla B_j(p) >
53 // = \int_R [ \nabla \cdot ( B_i(p) \nabla B_j(p) ) - B_i(p) \Delta B_j(p) ]
54 // = \int_dR < N(p) , B_i(p) \nabla B_j(p) > + A_2[i,j]
55 // and the first integral is zero if either f_i is zero on the boundary dR or the derivative of B_i across the boundary is zero.
56 // However, for the constraints we have:
57 // b_1(i) = \int_R < \nabla B_i(p) , V(p) >
58 // = \int_R [ \nabla \cdot ( B_i(p) V(p) ) - B_i(p) \nabla \cdot V(p) ]
59 // = \int_dR < N(p) , B_i(p) V(p) > + b_2[i]
60 // In particular, this implies that if the B_i satisfy the Neumann boundary conditions (rather than Dirichlet),
61 // and V is not zero across the boundary, then the two constraints are different.
62 // Forcing the < V(p) , N(p) > = 0 on the boundary, by killing off the component of the vector-field in the normal direction
63 // (FORCE_NEUMANN_FIELD), makes the two systems equal, and the value of this flag should be immaterial.
64 // Note that under interpretation 1, we have:
65 // \sum_i b_1(i) = < \nabla \sum_ i B_i(p) , V(p) > = 0
66 // because the B_i's sum to one. However, in general, we could have
67 // \sum_i b_2(i) \neq 0.
68 // This could cause trouble because the constant functions are in the kernel of the matrix A, so CG will misbehave if the constraint
69 // has a non-zero DC term. (Again, forcing < V(p) , N(p) > = 0 along the boundary resolves this problem.)
70 
71 #define FORCE_NEUMANN_FIELD 1 // This flag forces the normal component across the boundary of the integration domain to be zero.
72 // This should be enabled if GRADIENT_DOMAIN_SOLUTION is not, so that CG doesn't run into trouble.
73 
74 
75 #include <unordered_map>
76 
77 #include "bspline_data.h"
78 
79 
80 namespace pcl
81 {
82  namespace poisson
83  {
84 
85  typedef float Real;
86  typedef float BSplineDataReal;
88 
89 
90 
91  class RootInfo
92  {
93  public:
94  const TreeOctNode* node;
95  int edgeIndex;
96  long long key;
97  };
98 
99  class VertexData
100  {
101  public:
102  static long long EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth , int index[DIMENSION] );
103  static long long EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth );
104  static long long FaceIndex( const TreeOctNode* node , int fIndex , int maxDepth,int index[DIMENSION] );
105  static long long FaceIndex( const TreeOctNode* node , int fIndex , int maxDepth );
106  static long long CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int index[DIMENSION] );
107  static long long CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int index[DIMENSION] );
108  static long long CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth );
109  static long long CenterIndex( int depth , const int offSet[DIMENSION] , int maxDepth , int index[DIMENSION] );
110  static long long CenterIndex( const TreeOctNode* node , int maxDepth , int index[DIMENSION] );
111  static long long CenterIndex( const TreeOctNode* node , int maxDepth );
112  static long long CornerIndexKey( const int index[DIMENSION] );
113  };
115  {
116  public:
117  TreeOctNode** treeNodes;
118  int *nodeCount;
119  int maxDepth;
120  SortedTreeNodes( void );
121  ~SortedTreeNodes( void );
122  void set( TreeOctNode& root );
124  {
126  CornerIndices( void ) { memset( idx , -1 , sizeof( int ) * pcl::poisson::Cube::CORNERS ); }
127  int& operator[] ( int i ) { return idx[i]; }
128  const int& operator[] ( int i ) const { return idx[i]; }
129  };
131  {
132  CornerTableData( void ) { cCount=0; }
133  ~CornerTableData( void ) { clear(); }
134  void clear( void ) { cTable.clear() ; cCount = 0; }
135  CornerIndices& operator[] ( const TreeOctNode* node );
136  const CornerIndices& operator[] ( const TreeOctNode* node ) const;
137  CornerIndices& cornerIndices( const TreeOctNode* node );
138  const CornerIndices& cornerIndices( const TreeOctNode* node ) const;
139  int cCount;
140  std::vector< CornerIndices > cTable;
141  std::vector< int > offsets;
142  };
143  void setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int depth , int threads ) const;
144  void setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int threads ) const { setCornerTable( cData , rootNode , maxDepth-1 , threads ); }
145  void setCornerTable( CornerTableData& cData , int threads ) const { setCornerTable( cData , treeNodes[0] , maxDepth-1 , threads ); }
146  int getMaxCornerCount( const TreeOctNode* rootNode , int depth , int maxDepth , int threads ) const ;
147  struct EdgeIndices
148  {
150  EdgeIndices( void ) { memset( idx , -1 , sizeof( int ) * pcl::poisson::Cube::EDGES ); }
151  int& operator[] ( int i ) { return idx[i]; }
152  const int& operator[] ( int i ) const { return idx[i]; }
153  };
155  {
156  EdgeTableData( void ) { eCount=0; }
157  ~EdgeTableData( void ) { clear(); }
158  void clear( void ) { eTable.clear() , eCount=0; }
159  EdgeIndices& operator[] ( const TreeOctNode* node );
160  const EdgeIndices& operator[] ( const TreeOctNode* node ) const;
161  EdgeIndices& edgeIndices( const TreeOctNode* node );
162  const EdgeIndices& edgeIndices( const TreeOctNode* node ) const;
163  int eCount;
164  std::vector< EdgeIndices > eTable;
165  std::vector< int > offsets;
166  };
167  void setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int depth , int threads );
168  void setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int threads ) { setEdgeTable( eData , rootNode , maxDepth-1 , threads ); }
169  void setEdgeTable( EdgeTableData& eData , int threads ) { setEdgeTable( eData , treeNodes[0] , maxDepth-1 , threads ); }
170  int getMaxEdgeCount( const TreeOctNode* rootNode , int depth , int threads ) const ;
171  };
172 
174  {
175  public:
176  static int UseIndex;
178  union
179  {
180  int mcIndex;
181  struct
182  {
185  };
186  };
187  Real constraint , solution;
189 
190  TreeNodeData(void);
191  ~TreeNodeData(void);
192  };
193 
194  template< int Degree >
195  class Octree
196  {
197  SortedTreeNodes _sNodes;
198  int _minDepth;
199  bool _constrainValues;
200  std::vector< int > _pointCount;
201  struct PointData
202  {
204  Real weight;
205  Real value;
206  PointData( pcl::poisson::Point3D< Real > p , Real w , Real v=0 ) { position = p , weight = w , value = v; }
207  };
208  std::vector< PointData > _points;
209  TreeOctNode::NeighborKey3 neighborKey;
210  TreeOctNode::ConstNeighborKey3 neighborKey2;
211 
212  Real radius;
213  int width;
214  Real GetLaplacian( const int index[DIMENSION] ) const;
215  // Note that this is a slight misnomer. We're only taking the diveregence/Laplacian in the weak sense, so there is a change of sign.
216  Real GetLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 ) const;
217  Real GetDivergence( const TreeOctNode* node1 , const TreeOctNode* node2 , const pcl::poisson::Point3D<Real>& normal1 ) const;
218  Real GetDivergenceMinusLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 , Real value1 , const pcl::poisson::Point3D<Real>& normal1 ) const;
219  struct PointInfo
220  {
221  float splineValues[3][3];
222  float weightedValue;
223  };
224  Real GetValue( const PointInfo points[3][3][3] , const bool hasPoints[3][3] , const int d[3] ) const;
225 
226  class AdjacencyCountFunction
227  {
228  public:
229  int adjacencyCount;
230  void Function(const TreeOctNode* node1,const TreeOctNode* node2);
231  };
232  class AdjacencySetFunction{
233  public:
234  int *adjacencies,adjacencyCount;
235  void Function(const TreeOctNode* node1,const TreeOctNode* node2);
236  };
237 
238  class RefineFunction{
239  public:
240  int depth;
241  void Function(TreeOctNode* node1,const TreeOctNode* node2);
242  };
243  class FaceEdgesFunction
244  {
245  public:
246  int fIndex , maxDepth;
247  std::vector< std::pair< RootInfo , RootInfo > >* edges;
248  std::unordered_map< long long , std::pair< RootInfo , int > >* vertexCount;
249  void Function( const TreeOctNode* node1 , const TreeOctNode* node2 );
250  };
251 
252  int SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* subConstraints , bool showResidual , int minIters , double accuracy );
253  int SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* subConstraints , int startingDepth , bool showResidual , int minIters , double accuracy );
254 
255  void SetMatrixRowBounds( const TreeOctNode* node , int rDepth , const int rOff[3] , int& xStart , int& xEnd , int& yStart , int& yEnd , int& zStart , int& zEnd ) const;
256  int GetMatrixRowSize( const TreeOctNode::Neighbors5& neighbors5 ) const;
257  int GetMatrixRowSize( const TreeOctNode::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const;
258  int SetMatrixRow( const TreeOctNode::Neighbors5& neighbors5 , pcl::poisson::MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const;
259  int SetMatrixRow( const TreeOctNode::Neighbors5& neighbors5 , pcl::poisson::MatrixEntry< float >* row , int offset , const double stencil[5][5][5] , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const;
260  void SetDivergenceStencil( int depth , pcl::poisson::Point3D< double > *stencil , bool scatter ) const;
261  void SetLaplacianStencil( int depth , double stencil[5][5][5] ) const;
262  template< class C , int N > struct Stencil{ C values[N][N][N]; };
263  void SetLaplacianStencils( int depth , Stencil< double , 5 > stencil[2][2][2] ) const;
264  void SetDivergenceStencils( int depth , Stencil< pcl::poisson::Point3D< double > , 5 > stencil[2][2][2] , bool scatter ) const;
265  void SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const;
266 
267  static void UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ );
268  void UpdateConstraintsFromCoarser( const TreeOctNode::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& stencil ) const;
269  void SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution );
270  Real WeightedCoarserFunctionValue( const TreeOctNode::NeighborKey3& neighborKey3 , const TreeOctNode* node , Real* metSolution ) const;
271  void UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , pcl::poisson::Vector< Real >& solution ) const;
272  void DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const;
273  template< class C > void DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const;
274  template< class C > void UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const;
275  int GetFixedDepthLaplacian( pcl::poisson::SparseSymmetricMatrix<float>& matrix , int depth , const SortedTreeNodes& sNodes , Real* subConstraints );
276  int GetRestrictedFixedDepthLaplacian( pcl::poisson::SparseSymmetricMatrix<float>& matrix , int depth , const int* entries , int entryCount , const TreeOctNode* rNode, Real radius , const SortedTreeNodes& sNodes , Real* subConstraints );
277 
278  void SetIsoCorners( Real isoValue , TreeOctNode* leaf , SortedTreeNodes::CornerTableData& cData , char* valuesSet , Real* values , TreeOctNode::ConstNeighborKey3& nKey , const Real* metSolution , const Stencil< double , 3 > stencil1[8] , const Stencil< double , 3 > stencil2[8][8] );
279  static int IsBoundaryFace( const TreeOctNode* node , int faceIndex , int subdivideDepth );
280  static int IsBoundaryEdge( const TreeOctNode* node , int edgeIndex , int subdivideDepth );
281  static int IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subidivideDepth );
282 
283  // For computing the iso-surface there is a lot of re-computation of information across shared geometry.
284  // For function values we don't care so much.
285  // For edges we need to be careful so that the mesh remains water-tight
286  struct RootData : public SortedTreeNodes::CornerTableData , public SortedTreeNodes::EdgeTableData
287  {
288  // Edge to iso-vertex map
289  std::unordered_map< long long , int > boundaryRoots;
290  // Vertex to ( value , normal ) map
291  std::unordered_map< long long , std::pair< Real , pcl::poisson::Point3D< Real > > > *boundaryValues;
292  int* interiorRoots;
293  Real *cornerValues;
294  pcl::poisson::Point3D< Real >* cornerNormals;
295  char *cornerValuesSet , *cornerNormalsSet , *edgesSet;
296  };
297 
298  int SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , pcl::poisson::CoredMeshData* mesh , int nonLinearFit );
299  int SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData ,
300  std::vector< pcl::poisson::Point3D< float > >* interiorPositions , pcl::poisson::CoredMeshData* mesh , const Real* metSolution , int nonLinearFit );
301 #if MISHA_DEBUG
302  int GetMCIsoTriangles( TreeOctNode* node , pcl::poisson::CoredMeshData* mesh , RootData& rootData ,
303  std::vector< pcl::poisson::Point3D< float > >* interiorPositions , int offSet , int sDepth , bool polygonMesh , std::vector< pcl::poisson::Point3D< float > >* barycenters );
304  static int AddTriangles( pcl::poisson::CoredMeshData* mesh , std::vector<pcl::poisson::CoredPointIndex>& edges , std::vector<pcl::poisson::Point3D<float> >* interiorPositions , int offSet , bool polygonMesh , std::vector< pcl::poisson::Point3D< float > >* barycenters );
305 #else // !MISHA_DEBUG
306  int GetMCIsoTriangles( TreeOctNode* node , pcl::poisson::CoredMeshData* mesh , RootData& rootData ,
307  std::vector< pcl::poisson::Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh );
308  static int AddTriangles( pcl::poisson::CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh );
309 #endif // MISHA_DEBUG
310 
311 
312  void GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges );
313  static int GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops);
314  static int InteriorFaceRootCount( const TreeOctNode* node , const int &faceIndex , int maxDepth );
315  static int EdgeRootCount( const TreeOctNode* node , int edgeIndex , int maxDepth );
316  static void GetRootSpan( const RootInfo& ri , pcl::poisson::Point3D< float >& start , pcl::poisson::Point3D< float >& end );
317  int GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , pcl::poisson::Point3D<Real> & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit );
318  static int GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri );
319  static int GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth , RootInfo& ri );
320  static int GetRootIndex( const RootInfo& ri , RootData& rootData , pcl::poisson::CoredPointIndex& index );
321  static int GetRootPair( const RootInfo& root , int maxDepth , RootInfo& pair );
322 
323  int NonLinearUpdateWeightContribution(TreeOctNode* node,const pcl::poisson::Point3D<Real>& position,Real weight=Real(1.0));
324  Real NonLinearGetSampleWeight(TreeOctNode* node,const pcl::poisson::Point3D<Real>& position);
325  void NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const pcl::poisson::Point3D<Real>& position,Real samplesPerNode,Real& depth,Real& weight);
326  int NonLinearSplatOrientedPoint(TreeOctNode* node,const pcl::poisson::Point3D<Real>& point,const pcl::poisson::Point3D<Real>& normal);
327  Real NonLinearSplatOrientedPoint(const pcl::poisson::Point3D<Real>& point,const pcl::poisson::Point3D<Real>& normal , int kernelDepth , Real samplesPerNode , int minDepth , int maxDepth);
328 
329  int HasNormals(TreeOctNode* node,Real epsilon);
330  Real getCornerValue( const TreeOctNode::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution );
331  pcl::poisson::Point3D< Real > getCornerNormal( const TreeOctNode::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution );
332  Real getCornerValue( const TreeOctNode::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution , const double stencil1[3][3][3] , const double stencil2[3][3][3] );
333  Real getCenterValue( const TreeOctNode::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node );
334  public:
335  int threads;
336  static double maxMemoryUsage;
337  static double MemoryUsage( void );
338  std::vector< pcl::poisson::Point3D<Real> >* normals;
340  TreeOctNode tree;
342  Octree( void );
343 
344  void setBSplineData( int maxDepth , Real normalSmooth=-1 , bool reflectBoundary=false );
345  void finalize( void );
346  void RefineBoundary( int subdivisionDepth );
347  Real* GetWeightGrid( int& res , int depth=-1 );
348  Real* GetSolutionGrid( int& res , float isoValue=0.f , int depth=-1 );
349 
350  template<typename PointNT> int
351  setTree(typename pcl::PointCloud<PointNT>::ConstPtr input_ , int maxDepth , int minDepth ,
352  int kernelDepth , Real samplesPerNode , Real scaleFactor , Point3D<Real>& center , Real& scale ,
353  int useConfidence , Real constraintWeight , bool adaptiveWeights );
354 
355  void SetLaplacianConstraints(void);
356  void ClipTree(void);
357  int LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy );
358 
359  Real GetIsoValue(void);
360  void GetMCIsoTriangles( Real isoValue , int subdivideDepth , pcl::poisson::CoredMeshData* mesh , int fullDepthIso=0 , int nonLinearFit=1 , bool addBarycenter=false , bool polygonMesh=false );
361  };
362 
363 
364  }
365 }
366 
367 
368 
369 
370 #include "multi_grid_octree_data.hpp"
371 #endif // MULTI_GRID_OCTREE_DATA_INCLUDED
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Definition: sparse_matrix.h:45
void setEdgeTable(EdgeTableData &eData, int threads)
void setEdgeTable(EdgeTableData &eData, const TreeOctNode *rootNode, int threads)
void setCornerTable(CornerTableData &cData, const TreeOctNode *rootNode, int threads) const
pcl::poisson::BSplineData< Degree, BSplineDataReal > fData
pcl::poisson::OctNode< class TreeNodeData, Real > TreeOctNode
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
void setCornerTable(CornerTableData &cData, int threads) const
std::vector< pcl::poisson::Point3D< Real > > * normals