Point Cloud Library (PCL)  1.7.1
function_data.hpp
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 //////////////////
30 // FunctionData //
31 //////////////////
32 
33 namespace pcl
34 {
35  namespace poisson
36  {
37 
38  template<int Degree,class Real>
40  {
41  dotTable=dDotTable=d2DotTable=NULL;
42  valueTables=dValueTables=NULL;
43  res=0;
44  }
45 
46  template<int Degree,class Real>
48  {
49  if(res)
50  {
51  if( dotTable) delete[] dotTable;
52  if( dDotTable) delete[] dDotTable;
53  if(d2DotTable) delete[] d2DotTable;
54  if( valueTables) delete[] valueTables;
55  if(dValueTables) delete[] dValueTables;
56  }
57  dotTable=dDotTable=d2DotTable=NULL;
58  valueTables=dValueTables=NULL;
59  res=0;
60  }
61 
62  template<int Degree,class Real>
63 #if BOUNDARY_CONDITIONS
64  void FunctionData<Degree,Real>::set( const int& maxDepth , const PPolynomial<Degree>& F , const int& normalize , bool useDotRatios , bool reflectBoundary )
65 #else // !BOUNDARY_CONDITIONS
66  void FunctionData<Degree,Real>::set(const int& maxDepth,const PPolynomial<Degree>& F,const int& normalize , bool useDotRatios )
67 #endif // BOUNDARY_CONDITIONS
68  {
69  this->normalize = normalize;
70  this->useDotRatios = useDotRatios;
71 #if BOUNDARY_CONDITIONS
72  this->reflectBoundary = reflectBoundary;
73 #endif // BOUNDARY_CONDITIONS
74 
75  depth = maxDepth;
77  res2 = (1<<(depth+1))+1;
78  baseFunctions = new PPolynomial<Degree+1>[res];
79  // Scale the function so that it has:
80  // 0] Value 1 at 0
81  // 1] Integral equal to 1
82  // 2] Square integral equal to 1
83  switch( normalize )
84  {
85  case 2:
86  baseFunction=F/sqrt((F*F).integral(F.polys[0].start,F.polys[F.polyCount-1].start));
87  break;
88  case 1:
89  baseFunction=F/F.integral(F.polys[0].start,F.polys[F.polyCount-1].start);
90  break;
91  default:
92  baseFunction=F/F(0);
93  }
94  dBaseFunction = baseFunction.derivative();
95 #if BOUNDARY_CONDITIONS
96  leftBaseFunction = baseFunction + baseFunction.shift( -1 );
97  rightBaseFunction = baseFunction + baseFunction.shift( 1 );
98  dLeftBaseFunction = leftBaseFunction.derivative();
99  dRightBaseFunction = rightBaseFunction.derivative();
100 #endif // BOUNDARY_CONDITIONS
101  double c1,w1;
102  for( int i=0 ; i<res ; i++ )
103  {
105 #if BOUNDARY_CONDITIONS
106  if( reflectBoundary )
107  {
108  int d , off;
110  if ( off==0 ) baseFunctions[i] = leftBaseFunction.scale( w1 ).shift( c1 );
111  else if( off==((1<<d)-1) ) baseFunctions[i] = rightBaseFunction.scale( w1 ).shift( c1 );
112  else baseFunctions[i] = baseFunction.scale( w1 ).shift( c1 );
113  }
114  else baseFunctions[i] = baseFunction.scale(w1).shift(c1);
115 #else // !BOUNDARY_CONDITIONS
116  baseFunctions[i] = baseFunction.scale(w1).shift(c1);
117 #endif // BOUNDARY_CONDITIONS
118  // Scale the function so that it has L2-norm equal to one
119  switch( normalize )
120  {
121  case 2:
122  baseFunctions[i]/=sqrt(w1);
123  break;
124  case 1:
125  baseFunctions[i]/=w1;
126  break;
127  }
128  }
129  }
130  template<int Degree,class Real>
132  {
133  clearDotTables( flags );
134  int size;
135  size = ( res*res + res )>>1;
136  if( flags & DOT_FLAG )
137  {
138  dotTable = new Real[size];
139  memset( dotTable , 0 , sizeof(Real)*size );
140  }
141  if( flags & D_DOT_FLAG )
142  {
143  dDotTable = new Real[size];
144  memset( dDotTable , 0 , sizeof(Real)*size );
145  }
146  if( flags & D2_DOT_FLAG )
147  {
148  d2DotTable = new Real[size];
149  memset( d2DotTable , 0 , sizeof(Real)*size );
150  }
151  double t1 , t2;
152  t1 = baseFunction.polys[0].start;
153  t2 = baseFunction.polys[baseFunction.polyCount-1].start;
154  for( int i=0 ; i<res ; i++ )
155  {
156  double c1 , c2 , w1 , w2;
157  BinaryNode<double>::CenterAndWidth( i , c1 , w1 );
158 #if BOUNDARY_CONDITIONS
159  int d1 , d2 , off1 , off2;
160  BinaryNode< double >::DepthAndOffset( i , d1 , off1 );
161  int boundary1 = 0;
162  if ( reflectBoundary && off1==0 ) boundary1 = -1;
163  else if( reflectBoundary && off1==( (1<<d1)-1 ) ) boundary1 = 1;
164 #endif // BOUNDARY_CONDITIONS
165  double start1 = t1 * w1 + c1;
166  double end1 = t2 * w1 + c1;
167  for( int j=0 ; j<=i ; j++ )
168  {
169  BinaryNode<double>::CenterAndWidth( j , c2 , w2 );
170 #if BOUNDARY_CONDITIONS
171  BinaryNode< double >::DepthAndOffset( j , d2 , off2 );
172  int boundary2 = 0;
173  if ( reflectBoundary && off2==0 ) boundary2 = -1;
174  else if( reflectBoundary && off2==( (1<<d2)-1 ) ) boundary2 = 1;
175 #endif // BOUNDARY_CONDITIONS
176  int idx = SymmetricIndex( i , j );
177 
178  double start = t1 * w2 + c2;
179  double end = t2 * w2 + c2;
180 #if BOUNDARY_CONDITIONS
181  if( reflectBoundary )
182  {
183  if( start<0 ) start = 0;
184  if( start>1 ) start = 1;
185  if( end <0 ) end = 0;
186  if( end >1 ) end = 1;
187  }
188 #endif // BOUNDARY_CONDITIONS
189 
190  if( start< start1 ) start = start1;
191  if( end > end1 ) end = end1;
192  if( start>= end ) continue;
193 
194 #if BOUNDARY_CONDITIONS
195  Real dot = dotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 );
196 #else // !BOUNDARY_CONDITIONS
197  Real dot = dotProduct( c1 , w1 , c2 , w2 );
198 #endif // BOUNDARY_CONDITIONS
199  if( fabs(dot)<1e-15 ) continue;
200  if( flags & DOT_FLAG ) dotTable[idx]=dot;
201  if( useDotRatios )
202  {
203 #if BOUNDARY_CONDITIONS
204  if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot;
205  if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot;
206 #else // !BOUNDARY_CONDITIONS
207  if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct(c1,w1,c2,w2)/dot;
208  if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2)/dot;
209 #endif // BOUNDARY_CONDITIONS
210  }
211  else
212  {
213 #if BOUNDARY_CONDITIONS
214  if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 );
215  if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 );
216 #else // !BOUNDARY_CONDTIONS
217  if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct(c1,w1,c2,w2);
218  if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2);
219 #endif // BOUNDARY_CONDITIONS
220  }
221  }
222  }
223  }
224  template<int Degree,class Real>
226  {
227  if((flags & DOT_FLAG) && dotTable)
228  {
229  delete[] dotTable;
230  dotTable=NULL;
231  }
232  if((flags & D_DOT_FLAG) && dDotTable)
233  {
234  delete[] dDotTable;
235  dDotTable=NULL;
236  }
237  if((flags & D2_DOT_FLAG) && d2DotTable)
238  {
239  delete[] d2DotTable;
240  d2DotTable=NULL;
241  }
242  }
243  template<int Degree,class Real>
244  void FunctionData<Degree,Real>::setValueTables( const int& flags , const double& smooth )
245  {
246  clearValueTables();
247  if( flags & VALUE_FLAG ) valueTables = new Real[res*res2];
248  if( flags & D_VALUE_FLAG ) dValueTables = new Real[res*res2];
249  PPolynomial<Degree+1> function;
250  PPolynomial<Degree> dFunction;
251  for( int i=0 ; i<res ; i++ )
252  {
253  if(smooth>0)
254  {
255  function=baseFunctions[i].MovingAverage(smooth);
256  dFunction=baseFunctions[i].derivative().MovingAverage(smooth);
257  }
258  else
259  {
260  function=baseFunctions[i];
261  dFunction=baseFunctions[i].derivative();
262  }
263  for( int j=0 ; j<res2 ; j++ )
264  {
265  double x=double(j)/(res2-1);
266  if(flags & VALUE_FLAG){ valueTables[j*res+i]=Real( function(x));}
267  if(flags & D_VALUE_FLAG){dValueTables[j*res+i]=Real(dFunction(x));}
268  }
269  }
270  }
271  template<int Degree,class Real>
272  void FunctionData<Degree,Real>::setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth){
273  clearValueTables();
274  if(flags & VALUE_FLAG){ valueTables=new Real[res*res2];}
275  if(flags & D_VALUE_FLAG){dValueTables=new Real[res*res2];}
276  PPolynomial<Degree+1> function;
277  PPolynomial<Degree> dFunction;
278  for(int i=0;i<res;i++){
279  if(valueSmooth>0) { function=baseFunctions[i].MovingAverage(valueSmooth);}
280  else { function=baseFunctions[i];}
281  if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);}
282  else {dFunction=baseFunctions[i].derivative();}
283 
284  for(int j=0;j<res2;j++){
285  double x=double(j)/(res2-1);
286  if(flags & VALUE_FLAG){ valueTables[j*res+i]=Real( function(x));}
287  if(flags & D_VALUE_FLAG){dValueTables[j*res+i]=Real(dFunction(x));}
288  }
289  }
290  }
291 
292 
293  template<int Degree,class Real>
295  if( valueTables){delete[] valueTables;}
296  if(dValueTables){delete[] dValueTables;}
297  valueTables=dValueTables=NULL;
298  }
299 
300 #if BOUNDARY_CONDITIONS
301  template<int Degree,class Real>
302  Real FunctionData<Degree,Real>::dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const
303  {
304  const PPolynomial< Degree > *b1 , *b2;
305  if ( boundary1==-1 ) b1 = & leftBaseFunction;
306  else if( boundary1== 0 ) b1 = & baseFunction;
307  else if( boundary1== 1 ) b1 = &rightBaseFunction;
308  if ( boundary2==-1 ) b2 = & leftBaseFunction;
309  else if( boundary2== 0 ) b2 = & baseFunction;
310  else if( boundary2== 1 ) b2 = &rightBaseFunction;
311  double r=fabs( baseFunction.polys[0].start );
312  switch( normalize )
313  {
314  case 2:
315  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2));
316  case 1:
317  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2));
318  default:
319  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1);
320  }
321  }
322  template<int Degree,class Real>
323  Real FunctionData<Degree,Real>::dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const
324  {
325  const PPolynomial< Degree-1 > *b1;
326  const PPolynomial< Degree > *b2;
327  if ( boundary1==-1 ) b1 = & dLeftBaseFunction;
328  else if( boundary1== 0 ) b1 = & dBaseFunction;
329  else if( boundary1== 1 ) b1 = &dRightBaseFunction;
330  if ( boundary2==-1 ) b2 = & leftBaseFunction;
331  else if( boundary2== 0 ) b2 = & baseFunction;
332  else if( boundary2== 1 ) b2 = & rightBaseFunction;
333  double r=fabs(baseFunction.polys[0].start);
334  switch(normalize){
335  case 2:
336  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2));
337  case 1:
338  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2));
339  default:
340  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r));
341  }
342  }
343  template<int Degree,class Real>
344  Real FunctionData<Degree,Real>::d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const
345  {
346  const PPolynomial< Degree-1 > *b1 , *b2;
347  if ( boundary1==-1 ) b1 = & dLeftBaseFunction;
348  else if( boundary1== 0 ) b1 = & dBaseFunction;
349  else if( boundary1== 1 ) b1 = &dRightBaseFunction;
350  if ( boundary2==-1 ) b2 = & dLeftBaseFunction;
351  else if( boundary2== 0 ) b2 = & dBaseFunction;
352  else if( boundary2== 1 ) b2 = &dRightBaseFunction;
353  double r=fabs(baseFunction.polys[0].start);
354  switch( normalize )
355  {
356  case 2:
357  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2));
358  case 1:
359  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2));
360  default:
361  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2);
362  }
363  }
364 #else // !BOUNDARY_CONDITIONS
365  template<int Degree,class Real>
366  Real FunctionData<Degree,Real>::dotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{
367  double r=fabs(baseFunction.polys[0].start);
368  switch( normalize )
369  {
370  case 2:
371  return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2));
372  case 1:
373  return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2));
374  default:
375  return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1);
376  }
377  }
378  template<int Degree,class Real>
379  Real FunctionData<Degree,Real>::dDotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{
380  double r=fabs(baseFunction.polys[0].start);
381  switch(normalize){
382  case 2:
383  return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2));
384  case 1:
385  return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2));
386  default:
387  return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r));
388  }
389  }
390  template<int Degree,class Real>
391  Real FunctionData<Degree,Real>::d2DotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{
392  double r=fabs(baseFunction.polys[0].start);
393  switch(normalize){
394  case 2:
395  return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2));
396  case 1:
397  return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2));
398  default:
399  return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2);
400  }
401  }
402 #endif // BOUNDARY_CONDITIONS
403  template<int Degree,class Real>
404  inline int FunctionData<Degree,Real>::SymmetricIndex( const int& i1 , const int& i2 )
405  {
406  if( i1>i2 ) return ((i1*i1+i1)>>1)+i2;
407  else return ((i2*i2+i2)>>1)+i1;
408  }
409  template<int Degree,class Real>
410  inline int FunctionData<Degree,Real>::SymmetricIndex( const int& i1 , const int& i2 , int& index )
411  {
412  if( i1<i2 )
413  {
414  index = ((i2*i2+i2)>>1)+i1;
415  return 1;
416  }
417  else{
418  index = ((i1*i1+i1)>>1)+i2;
419  return 0;
420  }
421  }
422  }
423 }
virtual void clearDotTables(const int &flags)
virtual void setDotTables(const int &flags)
static int CumulativeCenterCount(int maxDepth)
Definition: binary_node.h:44
PPolynomial< Degree-1 > derivative(void) const
Real dDotProduct(const double &center1, const double &width1, const double &center2, const double &width2, int boundary1, int boundary2) const
Real d2DotProduct(const double &center1, const double &width1, const double &center2, const double &width2, int boundary1, int boundary2) const
static void DepthAndOffset(int idx, int &depth, int &offset)
Definition: binary_node.h:64
void set(const int &maxDepth, const PPolynomial< Degree > &F, const int &normalize, bool useDotRatios=true, bool reflectBoundary=false)
PPolynomial scale(double s) const
PPolynomial shift(double t) const
virtual void clearValueTables(void)
virtual void setValueTables(const int &flags, const double &smooth=0)
static int SymmetricIndex(const int &i1, const int &i2)
PPolynomial< Degree+1 > MovingAverage(double radius)
Real dotProduct(const double &center1, const double &width1, const double &center2, const double &width2, int boundary1, int boundary2) const
static void CenterAndWidth(int depth, int offset, Real &center, Real &width)
Definition: binary_node.h:53