Point Cloud Library (PCL)  1.14.0-dev
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>
39  FunctionData<Degree,Real>::FunctionData(void)
40  {
41  dotTable=dDotTable=d2DotTable=NULL;
42  valueTables=dValueTables=NULL;
43  res=0;
44  }
45 
46  template<int Degree,class Real>
47  FunctionData<Degree,Real>::~FunctionData(void)
48  {
49  if(res)
50  {
51  delete[] dotTable;
52  delete[] dDotTable;
53  delete[] d2DotTable;
54  delete[] valueTables;
55  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;
76  res = BinaryNode<double>::CumulativeCenterCount( depth );
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  {
104  BinaryNode< double >::CenterAndWidth( i , c1 , w1 );
105 #if BOUNDARY_CONDITIONS
106  if( reflectBoundary )
107  {
108  int d , off;
109  BinaryNode< double >::DepthAndOffset( i , 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>
131  void FunctionData<Degree,Real>::setDotTables( const int& flags )
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>
225  void FunctionData<Degree,Real>::clearDotTables( const int& flags )
226  {
227  if (flags & DOT_FLAG)
228  {
229  delete[] dotTable;
230  dotTable=NULL;
231  delete[] dDotTable;
232  dDotTable=NULL;
233  delete[] d2DotTable;
234  d2DotTable=NULL;
235  }
236  }
237  template<int Degree,class Real>
238  void FunctionData<Degree,Real>::setValueTables( const int& flags , const double& smooth )
239  {
240  clearValueTables();
241  if( flags & VALUE_FLAG ) valueTables = new Real[res*res2];
242  if( flags & D_VALUE_FLAG ) dValueTables = new Real[res*res2];
243  PPolynomial<Degree+1> function;
244  PPolynomial<Degree> dFunction;
245  for( int i=0 ; i<res ; i++ )
246  {
247  if(smooth>0)
248  {
249  function=baseFunctions[i].MovingAverage(smooth);
250  dFunction=baseFunctions[i].derivative().MovingAverage(smooth);
251  }
252  else
253  {
254  function=baseFunctions[i];
255  dFunction=baseFunctions[i].derivative();
256  }
257  for( int j=0 ; j<res2 ; j++ )
258  {
259  double x=double(j)/(res2-1);
260  if(flags & VALUE_FLAG){ valueTables[j*res+i]=Real( function(x));}
261  if(flags & D_VALUE_FLAG){dValueTables[j*res+i]=Real(dFunction(x));}
262  }
263  }
264  }
265  template<int Degree,class Real>
266  void FunctionData<Degree,Real>::setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth){
267  clearValueTables();
268  if(flags & VALUE_FLAG){ valueTables=new Real[res*res2];}
269  if(flags & D_VALUE_FLAG){dValueTables=new Real[res*res2];}
270  PPolynomial<Degree+1> function;
271  PPolynomial<Degree> dFunction;
272  for(int i=0;i<res;i++){
273  if(valueSmooth>0) { function=baseFunctions[i].MovingAverage(valueSmooth);}
274  else { function=baseFunctions[i];}
275  if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);}
276  else {dFunction=baseFunctions[i].derivative();}
277 
278  for(int j=0;j<res2;j++){
279  double x=double(j)/(res2-1);
280  if(flags & VALUE_FLAG){ valueTables[j*res+i]=Real( function(x));}
281  if(flags & D_VALUE_FLAG){dValueTables[j*res+i]=Real(dFunction(x));}
282  }
283  }
284  }
285 
286 
287  template<int Degree,class Real>
288  void FunctionData<Degree,Real>::clearValueTables(void){
289  delete[] valueTables;
290  delete[] dValueTables;
291  valueTables=dValueTables=NULL;
292  }
293 
294 #if BOUNDARY_CONDITIONS
295  template<int Degree,class Real>
296  Real FunctionData<Degree,Real>::dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const
297  {
298  const PPolynomial< Degree > *b1 , *b2;
299  if ( boundary1==-1 ) b1 = & leftBaseFunction;
300  else if( boundary1== 0 ) b1 = & baseFunction;
301  else if( boundary1== 1 ) b1 = &rightBaseFunction;
302  if ( boundary2==-1 ) b2 = & leftBaseFunction;
303  else if( boundary2== 0 ) b2 = & baseFunction;
304  else if( boundary2== 1 ) b2 = &rightBaseFunction;
305  double r=fabs( baseFunction.polys[0].start );
306  switch( normalize )
307  {
308  case 2:
309  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2));
310  case 1:
311  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2));
312  default:
313  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1);
314  }
315  }
316  template<int Degree,class Real>
317  Real FunctionData<Degree,Real>::dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const
318  {
319  const PPolynomial< Degree-1 > *b1;
320  const PPolynomial< Degree > *b2;
321  if ( boundary1==-1 ) b1 = & dLeftBaseFunction;
322  else if( boundary1== 0 ) b1 = & dBaseFunction;
323  else if( boundary1== 1 ) b1 = &dRightBaseFunction;
324  if ( boundary2==-1 ) b2 = & leftBaseFunction;
325  else if( boundary2== 0 ) b2 = & baseFunction;
326  else if( boundary2== 1 ) b2 = & rightBaseFunction;
327  double r=fabs(baseFunction.polys[0].start);
328  switch(normalize){
329  case 2:
330  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2));
331  case 1:
332  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2));
333  default:
334  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r));
335  }
336  }
337  template<int Degree,class Real>
338  Real FunctionData<Degree,Real>::d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const
339  {
340  const PPolynomial< Degree-1 > *b1 , *b2;
341  if ( boundary1==-1 ) b1 = & dLeftBaseFunction;
342  else if( boundary1== 0 ) b1 = & dBaseFunction;
343  else if( boundary1== 1 ) b1 = &dRightBaseFunction;
344  if ( boundary2==-1 ) b2 = & dLeftBaseFunction;
345  else if( boundary2== 0 ) b2 = & dBaseFunction;
346  else if( boundary2== 1 ) b2 = &dRightBaseFunction;
347  double r=fabs(baseFunction.polys[0].start);
348  switch( normalize )
349  {
350  case 2:
351  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2));
352  case 1:
353  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2));
354  default:
355  return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2);
356  }
357  }
358 #else // !BOUNDARY_CONDITIONS
359  template<int Degree,class Real>
360  Real FunctionData<Degree,Real>::dotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{
361  double r=fabs(baseFunction.polys[0].start);
362  switch( normalize )
363  {
364  case 2:
365  return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2));
366  case 1:
367  return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2));
368  default:
369  return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1);
370  }
371  }
372  template<int Degree,class Real>
373  Real FunctionData<Degree,Real>::dDotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{
374  double r=fabs(baseFunction.polys[0].start);
375  switch(normalize){
376  case 2:
377  return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2));
378  case 1:
379  return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2));
380  default:
381  return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r));
382  }
383  }
384  template<int Degree,class Real>
385  Real FunctionData<Degree,Real>::d2DotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{
386  double r=fabs(baseFunction.polys[0].start);
387  switch(normalize){
388  case 2:
389  return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2));
390  case 1:
391  return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2));
392  default:
393  return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2);
394  }
395  }
396 #endif // BOUNDARY_CONDITIONS
397  template<int Degree,class Real>
398  inline int FunctionData<Degree,Real>::SymmetricIndex( const int& i1 , const int& i2 )
399  {
400  if( i1>i2 ) return ((i1*i1+i1)>>1)+i2;
401  else return ((i2*i2+i2)>>1)+i1;
402  }
403  template<int Degree,class Real>
404  inline int FunctionData<Degree,Real>::SymmetricIndex( const int& i1 , const int& i2 , int& index )
405  {
406  if( i1<i2 )
407  {
408  index = ((i2*i2+i2)>>1)+i1;
409  return 1;
410  }
411  else{
412  index = ((i1*i1+i1)>>1)+i2;
413  return 0;
414  }
415  }
416  }
417 }
pcl::poisson::BinaryNode::CenterAndWidth
static void CenterAndWidth(int depth, int offset, Real &center, Real &width)
Definition: binary_node.h:53
pcl::poisson::BinaryNode::CumulativeCenterCount
static int CumulativeCenterCount(int maxDepth)
Definition: binary_node.h:44
pcl::poisson::BinaryNode::DepthAndOffset
static void DepthAndOffset(int idx, int &depth, int &offset)
Definition: binary_node.h:64
pcl::poisson::FunctionData::setDotTables
virtual void setDotTables(const int &flags)
Definition: function_data.hpp:131
pcl::poisson::FunctionData::clearValueTables
virtual void clearValueTables(void)
Definition: function_data.hpp:288
pcl::poisson::FunctionData::dDotProduct
Real dDotProduct(const double &center1, const double &width1, const double &center2, const double &width2, int boundary1, int boundary2) const
Definition: function_data.hpp:373
pcl::poisson::FunctionData::SymmetricIndex
static int SymmetricIndex(const int &i1, const int &i2)
Definition: function_data.hpp:398
pcl::poisson::FunctionData::setValueTables
virtual void setValueTables(const int &flags, const double &smooth=0)
Definition: function_data.hpp:238
pcl::poisson::FunctionData::set
void set(const int &maxDepth, const PPolynomial< Degree > &F, const int &normalize, bool useDotRatios=true, bool reflectBoundary=false)
Definition: function_data.hpp:66
pcl::poisson::FunctionData::d2DotProduct
Real d2DotProduct(const double &center1, const double &width1, const double &center2, const double &width2, int boundary1, int boundary2) const
Definition: function_data.hpp:385
pcl::poisson::FunctionData::dotProduct
Real dotProduct(const double &center1, const double &width1, const double &center2, const double &width2, int boundary1, int boundary2) const
Definition: function_data.hpp:360
pcl::poisson::FunctionData::clearDotTables
virtual void clearDotTables(const int &flags)
Definition: function_data.hpp:225
pcl::poisson::PPolynomial::shift
PPolynomial shift(double t) const
Definition: ppolynomial.hpp:279
pcl::poisson::PPolynomial::scale
PPolynomial scale(double s) const
Definition: ppolynomial.hpp:271
pcl::poisson::PPolynomial::derivative
PPolynomial< Degree-1 > derivative(void) const
Definition: ppolynomial.hpp:287
pcl::poisson::PPolynomial::MovingAverage
PPolynomial< Degree+1 > MovingAverage(double radius)
Definition: ppolynomial.hpp:396