Point Cloud Library (PCL)  1.14.1-dev
bspline_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 #include "poisson_exceptions.h"
30 #include "binary_node.h"
31 
32 namespace pcl
33 {
34  namespace poisson
35  {
36 
37  /////////////////
38  // BSplineData //
39  /////////////////
40  // Support[i]:
41  // Odd: i +/- 0.5 * ( 1 + Degree )
42  // i - 0.5 * ( 1 + Degree ) < 0
43  // <=> i < 0.5 * ( 1 + Degree )
44  // i + 0.5 * ( 1 + Degree ) > 0
45  // <=> i > - 0.5 * ( 1 + Degree )
46  // i + 0.5 * ( 1 + Degree ) > r
47  // <=> i > r - 0.5 * ( 1 + Degree )
48  // i - 0.5 * ( 1 + Degree ) < r
49  // <=> i < r + 0.5 * ( 1 + Degree )
50  // Even: i + 0.5 +/- 0.5 * ( 1 + Degree )
51  // i - 0.5 * Degree < 0
52  // <=> i < 0.5 * Degree
53  // i + 1 + 0.5 * Degree > 0
54  // <=> i > -1 - 0.5 * Degree
55  // i + 1 + 0.5 * Degree > r
56  // <=> i > r - 1 - 0.5 * Degree
57  // i - 0.5 * Degree < r
58  // <=> i < r + 0.5 * Degree
59  template< int Degree > inline bool LeftOverlap( unsigned int, int offset )
60  {
61  offset <<= 1;
62  if( Degree & 1 ) return (offset < 1+Degree) && (offset > -1-Degree );
63  else return (offset < Degree) && (offset > -2-Degree );
64  }
65  template< int Degree > inline bool RightOverlap( unsigned int, int offset )
66  {
67  offset <<= 1;
68  if( Degree & 1 ) return (offset > 2-1-Degree) && (offset < 2+1+Degree );
69  else return (offset > 2-2-Degree) && (offset < 2+ Degree );
70  }
71  template< int Degree > inline int ReflectLeft( unsigned int, int offset )
72  {
73  if( Degree&1 ) return -offset;
74  else return -1-offset;
75  }
76  template< int Degree > inline int ReflectRight( unsigned int depth , int offset )
77  {
78  int r = 1<<(depth+1);
79  if( Degree&1 ) return r -offset;
80  else return r-1-offset;
81  }
82 
83  template< int Degree , class Real >
85  {
86  vvDotTable = dvDotTable = ddDotTable = NULL;
87  valueTables = dValueTables = NULL;
88  baseFunctions = NULL;
89  baseBSplines = NULL;
90  functionCount = sampleCount = 0;
91  }
92 
93  template< int Degree , class Real >
95  {
96  if( functionCount )
97  {
98  delete[] vvDotTable;
99  delete[] dvDotTable;
100  delete[] ddDotTable;
101 
102  delete[] valueTables;
103  delete[] dValueTables;
104 
105  delete[] baseFunctions;
106  delete[] baseBSplines;
107  }
108  vvDotTable = dvDotTable = ddDotTable = NULL;
109  valueTables = dValueTables=NULL;
110  baseFunctions = NULL;
111  baseBSplines = NULL;
112  functionCount = 0;
113  }
114 
115  template<int Degree,class Real>
116  void BSplineData<Degree,Real>::set( int maxDepth , bool useDotRatios , bool reflectBoundary )
117  {
118  this->useDotRatios = useDotRatios;
119  this->reflectBoundary = reflectBoundary;
120 
121  depth = maxDepth;
122  // [Warning] This assumes that the functions spacing is dual
123  functionCount = BinaryNode< double >::CumulativeCenterCount( depth );
125  baseFunctions = new PPolynomial<Degree>[functionCount];
126  baseBSplines = new BSplineComponents[functionCount];
127 
128  baseFunction = PPolynomial< Degree >::BSpline();
129  for( int i=0 ; i<=Degree ; i++ ) baseBSpline[i] = Polynomial< Degree >::BSplineComponent( i ).shift( static_cast<double>(-(Degree+1)/2) + i - 0.5 );
130  dBaseFunction = baseFunction.derivative();
131  StartingPolynomial< Degree > sPolys[Degree+3];
132 
133  for( int i=0 ; i<Degree+3 ; i++ )
134  {
135  sPolys[i].start = static_cast<double>(-(Degree+1)/2) + i - 1.5;
136  sPolys[i].p *= 0;
137  if( i<=Degree ) sPolys[i].p += baseBSpline[i ].shift( -1 );
138  if( i>=1 && i<=Degree+1 ) sPolys[i].p += baseBSpline[i-1];
139  for( int j=0 ; j<i ; j++ ) sPolys[i].p -= sPolys[j].p;
140  }
141  leftBaseFunction.set( sPolys , Degree+3 );
142  for( int i=0 ; i<Degree+3 ; i++ )
143  {
144  sPolys[i].start = static_cast<double>(-(Degree+1)/2) + i - 0.5;
145  sPolys[i].p *= 0;
146  if( i<=Degree ) sPolys[i].p += baseBSpline[i ];
147  if( i>=1 && i<=Degree+1 ) sPolys[i].p += baseBSpline[i-1].shift( 1 );
148  for( int j=0 ; j<i ; j++ ) sPolys[i].p -= sPolys[j].p;
149  }
150  rightBaseFunction.set( sPolys , Degree+3 );
151  dLeftBaseFunction = leftBaseFunction.derivative();
152  dRightBaseFunction = rightBaseFunction.derivative();
153  leftBSpline = rightBSpline = baseBSpline;
154  leftBSpline [1] += leftBSpline[2].shift( -1 ) , leftBSpline[0] *= 0;
155  rightBSpline[1] += rightBSpline[0].shift( 1 ) , rightBSpline[2] *= 0;
156  double c , w;
157  for( int i=0 ; i<functionCount ; i++ )
158  {
160  baseFunctions[i] = baseFunction.scale(w).shift(c);
161  baseBSplines[i] = baseBSpline.scale(w).shift(c);
162  if( reflectBoundary )
163  {
164  int d , off , r;
166  r = 1<<d;
167  if ( off==0 ) baseFunctions[i] = leftBaseFunction.scale(w).shift(c);
168  else if( off==r-1 ) baseFunctions[i] = rightBaseFunction.scale(w).shift(c);
169  if ( off==0 ) baseBSplines[i] = leftBSpline.scale(w).shift(c);
170  else if( off==r-1 ) baseBSplines[i] = rightBSpline.scale(w).shift(c);
171  }
172  }
173  }
174  template<int Degree,class Real>
176  {
177  clearDotTables( flags );
178  int size = ( functionCount*functionCount + functionCount )>>1;
179  int fullSize = functionCount*functionCount;
180  if( flags & VV_DOT_FLAG )
181  {
182  vvDotTable = new Real[size]{};
183  }
184  if( flags & DV_DOT_FLAG )
185  {
186  dvDotTable = new Real[fullSize]{};
187  }
188  if( flags & DD_DOT_FLAG )
189  {
190  ddDotTable = new Real[size]{};
191  }
192  double vvIntegrals[Degree+1][Degree+1];
193  double vdIntegrals[Degree+1][Degree ];
194  double dvIntegrals[Degree ][Degree+1];
195  double ddIntegrals[Degree ][Degree ];
196  int vvSums[Degree+1][Degree+1];
197  int vdSums[Degree+1][Degree ];
198  int dvSums[Degree ][Degree+1];
199  int ddSums[Degree ][Degree ];
200  SetBSplineElementIntegrals< Degree , Degree >( vvIntegrals );
201  SetBSplineElementIntegrals< Degree , Degree-1 >( vdIntegrals );
202  SetBSplineElementIntegrals< Degree-1 , Degree >( dvIntegrals );
203  SetBSplineElementIntegrals< Degree-1 , Degree-1 >( ddIntegrals );
204 
205  for( int d1=0 ; d1<=depth ; d1++ )
206  for( int off1=0 ; off1<(1<<d1) ; off1++ )
207  {
208  int ii = BinaryNode< Real >::CenterIndex( d1 , off1 );
210  BSplineElements< Degree-1 > db1;
211  b1.differentiate( db1 );
212 
213  int start1 , end1;
214 
215  start1 = -1;
216  for( int i=0 ; i<int(b1.size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
217  {
218  if( b1[i][j] && start1==-1 ) start1 = i;
219  if( b1[i][j] ) end1 = i+1;
220  }
221  for( int d2=d1 ; d2<=depth ; d2++ )
222  {
223  for( int off2=0 ; off2<(1<<d2) ; off2++ )
224  {
225  int start2 = off2-Degree;
226  int end2 = off2+Degree+1;
227  if( start2>=end1 || start1>=end2 ) continue;
228  start2 = std::max< int >( start1 , start2 );
229  end2 = std::min< int >( end1 , end2 );
230  if( d1==d2 && off2<off1 ) continue;
231  int jj = BinaryNode< Real >::CenterIndex( d2 , off2 );
233  BSplineElements< Degree-1 > db2;
234  b2.differentiate( db2 );
235 
236  int idx = SymmetricIndex( ii , jj );
237  int idx1 = Index( ii , jj ) , idx2 = Index( jj , ii );
238 
239  memset( vvSums , 0 , sizeof( int ) * ( Degree+1 ) * ( Degree+1 ) );
240  memset( vdSums , 0 , sizeof( int ) * ( Degree+1 ) * ( Degree ) );
241  memset( dvSums , 0 , sizeof( int ) * ( Degree ) * ( Degree+1 ) );
242  memset( ddSums , 0 , sizeof( int ) * ( Degree ) * ( Degree ) );
243  for( int i=start2 ; i<end2 ; i++ )
244  {
245  for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) vvSums[j][k] += b1[i][j] * b2[i][k];
246  for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) vdSums[j][k] += b1[i][j] * db2[i][k];
247  for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) dvSums[j][k] += db1[i][j] * b2[i][k];
248  for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) ddSums[j][k] += db1[i][j] * db2[i][k];
249  }
250  double vvDot = 0 , dvDot = 0 , vdDot = 0 , ddDot = 0;
251  for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) vvDot += vvIntegrals[j][k] * vvSums[j][k];
252  for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) vdDot += vdIntegrals[j][k] * vdSums[j][k];
253  for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) dvDot += dvIntegrals[j][k] * dvSums[j][k];
254  for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) ddDot += ddIntegrals[j][k] * ddSums[j][k];
255  vvDot /= (1<<d2);
256  ddDot *= (1<<d2);
257  vvDot /= ( b1.denominator * b2.denominator );
258  dvDot /= ( b1.denominator * b2.denominator );
259  vdDot /= ( b1.denominator * b2.denominator );
260  ddDot /= ( b1.denominator * b2.denominator );
261  if( fabs(vvDot)<1e-15 ) continue;
262  if( flags & VV_DOT_FLAG ) vvDotTable [idx] = Real( vvDot );
263  if( useDotRatios )
264  {
265  if( flags & DV_DOT_FLAG ) dvDotTable[idx1] = Real( dvDot / vvDot );
266  if( flags & DV_DOT_FLAG ) dvDotTable[idx2] = Real( vdDot / vvDot );
267  if( flags & DD_DOT_FLAG ) ddDotTable[idx ] = Real( ddDot / vvDot );
268  }
269  else
270  {
271  if( flags & DV_DOT_FLAG ) dvDotTable[idx1] = Real( dvDot );
272  if( flags & DV_DOT_FLAG ) dvDotTable[idx2] = Real( dvDot );
273  if( flags & DD_DOT_FLAG ) ddDotTable[idx ] = Real( ddDot );
274  }
275  }
277  b = b1;
278  b.upSample( b1 );
279  b1.differentiate( db1 );
280  start1 = -1;
281  for( int i=0 ; i<int(b1.size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
282  {
283  if( b1[i][j] && start1==-1 ) start1 = i;
284  if( b1[i][j] ) end1 = i+1;
285  }
286  }
287  }
288  }
289  template<int Degree,class Real>
291  {
292  if (flags & VV_DOT_FLAG) {
293  delete[] vvDotTable ; vvDotTable = NULL;
294  }
295  if (flags & DV_DOT_FLAG) {
296  delete[] dvDotTable ; dvDotTable = NULL;
297  }
298  if (flags & DD_DOT_FLAG) {
299  delete[] ddDotTable ; ddDotTable = NULL;
300  }
301  }
302  template< int Degree , class Real >
303  void BSplineData< Degree , Real >::setSampleSpan( int idx , int& start , int& end , double smooth ) const
304  {
305  int d , off , res;
306  BinaryNode< double >::DepthAndOffset( idx , d , off );
307  res = 1<<d;
308  double _start = ( off + 0.5 - 0.5*(Degree+1) ) / res - smooth;
309  double _end = ( off + 0.5 + 0.5*(Degree+1) ) / res + smooth;
310  // (start)/(sampleCount-1) >_start && (start-1)/(sampleCount-1)<=_start
311  // => start > _start * (sampleCount-1 ) && start <= _start*(sampleCount-1) + 1
312  // => _start * (sampleCount-1) + 1 >= start > _start * (sampleCount-1)
313  start = static_cast<int>( floor( _start * (sampleCount-1) + 1 ) );
314  if( start<0 ) start = 0;
315  // (end)/(sampleCount-1)<_end && (end+1)/(sampleCount-1)>=_end
316  // => end < _end * (sampleCount-1 ) && end >= _end*(sampleCount-1) - 1
317  // => _end * (sampleCount-1) > end >= _end * (sampleCount-1) - 1
318  end = static_cast<int>( ceil( _end * (sampleCount-1) - 1 ) );
319  if( end>=sampleCount ) end = sampleCount-1;
320  }
321  template<int Degree,class Real>
322  void BSplineData<Degree,Real>::setValueTables( int flags , double smooth )
323  {
324  clearValueTables();
325  if( flags & VALUE_FLAG ) valueTables = new Real[functionCount*sampleCount];
326  if( flags & D_VALUE_FLAG ) dValueTables = new Real[functionCount*sampleCount];
327  PPolynomial<Degree+1> function;
328  PPolynomial<Degree> dFunction;
329  for( int i=0 ; i<functionCount ; i++ )
330  {
331  if(smooth>0)
332  {
333  function = baseFunctions[i].MovingAverage(smooth);
334  dFunction = baseFunctions[i].derivative().MovingAverage(smooth);
335  }
336  else
337  {
338  function = baseFunctions[i];
339  dFunction = baseFunctions[i].derivative();
340  }
341  for( int j=0 ; j<sampleCount ; j++ )
342  {
343  double x=static_cast<double>(j)/(sampleCount-1);
344  if(flags & VALUE_FLAG){ valueTables[j*functionCount+i]=Real( function(x));}
345  if(flags & D_VALUE_FLAG){dValueTables[j*functionCount+i]=Real(dFunction(x));}
346  }
347  }
348  }
349  template<int Degree,class Real>
350  void BSplineData<Degree,Real>::setValueTables(int flags,double valueSmooth,double normalSmooth){
351  clearValueTables();
352  if(flags & VALUE_FLAG){ valueTables=new Real[functionCount*sampleCount];}
353  if(flags & D_VALUE_FLAG){dValueTables=new Real[functionCount*sampleCount];}
354  PPolynomial<Degree+1> function;
355  PPolynomial<Degree> dFunction;
356  for(int i=0;i<functionCount;i++){
357  if(valueSmooth>0) { function=baseFunctions[i].MovingAverage(valueSmooth);}
358  else { function=baseFunctions[i];}
359  if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);}
360  else {dFunction=baseFunctions[i].derivative();}
361 
362  for(int j=0;j<sampleCount;j++){
363  double x=static_cast<double>(j)/(sampleCount-1);
364  if(flags & VALUE_FLAG){ valueTables[j*functionCount+i]=Real( function(x));}
365  if(flags & D_VALUE_FLAG){dValueTables[j*functionCount+i]=Real(dFunction(x));}
366  }
367  }
368  }
369 
370 
371  template<int Degree,class Real>
373  delete[] valueTables;
374  delete[] dValueTables;
375  valueTables=dValueTables=NULL;
376  }
377 
378  template<int Degree,class Real>
379  inline int BSplineData<Degree,Real>::Index( int i1 , int i2 ) const { return i1*functionCount+i2; }
380  template<int Degree,class Real>
381  inline int BSplineData<Degree,Real>::SymmetricIndex( int i1 , int i2 )
382  {
383  if( i1>i2 ) return ((i1*i1+i1)>>1)+i2;
384  else return ((i2*i2+i2)>>1)+i1;
385  }
386  template<int Degree,class Real>
387  inline int BSplineData<Degree,Real>::SymmetricIndex( int i1 , int i2 , int& index )
388  {
389  if( i1<i2 )
390  {
391  index = ((i2*i2+i2)>>1)+i1;
392  return 1;
393  }
394  else
395  {
396  index = ((i1*i1+i1)>>1)+i2;
397  return 0;
398  }
399  }
400 
401 
402  ////////////////////////
403  // BSplineElementData //
404  ////////////////////////
405  template< int Degree >
406  BSplineElements< Degree >::BSplineElements( int res , int offset , int boundary )
407  {
408  denominator = 1;
409  this->resize( res , BSplineElementCoefficients<Degree>() );
410 
411  for( int i=0 ; i<=Degree ; i++ )
412  {
413  int idx = -_off + offset + i;
414  if( idx>=0 && idx<res ) (*this)[idx][i] = 1;
415  }
416  if( boundary!=0 )
417  {
418  _addLeft( offset-2*res , boundary ) , _addRight( offset+2*res , boundary );
419  if( Degree&1 ) _addLeft( offset-res , boundary ) , _addRight( offset+res , boundary );
420  else _addLeft( -offset-1 , boundary ) , _addRight( -offset-1+2*res , boundary );
421  }
422  }
423  template< int Degree >
424  void BSplineElements< Degree >::_addLeft( int offset , int boundary )
425  {
426  int res = int( this->size() );
427  bool set = false;
428  for( int i=0 ; i<=Degree ; i++ )
429  {
430  int idx = -_off + offset + i;
431  if( idx>=0 && idx<res ) (*this)[idx][i] += boundary , set = true;
432  }
433  if( set ) _addLeft( offset-2*res , boundary );
434  }
435  template< int Degree >
436  void BSplineElements< Degree >::_addRight( int offset , int boundary )
437  {
438  int res = int( this->size() );
439  bool set = false;
440  for( int i=0 ; i<=Degree ; i++ )
441  {
442  int idx = -_off + offset + i;
443  if( idx>=0 && idx<res ) (*this)[idx][i] += boundary , set = true;
444  }
445  if( set ) _addRight( offset+2*res , boundary );
446  }
447  template< int Degree >
449  {
450  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "B-spline up-sampling not supported for degree " << Degree);
451  }
452  template<>
454 
455  template<>
457 
458  template< int Degree >
460  {
461  d.resize( this->size() );
462  d.assign( d.size() , BSplineElementCoefficients< Degree-1 >() );
463  for( int i=0 ; i<int(this->size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
464  {
465  if( j-1>=0 ) d[i][j-1] -= (*this)[i][j];
466  if( j<Degree ) d[i][j ] += (*this)[i][j];
467  }
468  d.denominator = denominator;
469  }
470  // If we were really good, we would implement this integral table to store
471  // rational values to improve precision...
472  template< int Degree1 , int Degree2 >
473  void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] )
474  {
475  for( int i=0 ; i<=Degree1 ; i++ )
476  {
478  for( int j=0 ; j<=Degree2 ; j++ )
479  {
481  integrals[i][j] = ( p1 * p2 ).integral( 0 , 1 );
482  }
483  }
484  }
485 
486 
487  }
488 }
void set(int maxDepth, bool useDotRatios=true, bool reflectBoundary=false)
static int SymmetricIndex(int i1, int i2)
virtual void setValueTables(int flags, double smooth=0)
virtual void clearDotTables(int flags)
virtual void clearValueTables()
virtual void setDotTables(int flags)
void setSampleSpan(int idx, int &start, int &end, double smooth=0) const
int Index(int i1, int i2) const
static void CenterAndWidth(int depth, int offset, Real &center, Real &width)
Definition: binary_node.h:53
static int CumulativeCenterCount(int maxDepth)
Definition: binary_node.h:44
static int CenterIndex(int depth, int offSet)
Definition: binary_node.h:47
static int CornerCount(int depth)
Definition: binary_node.h:43
static int CenterCount(int depth)
Definition: binary_node.h:42
static void DepthAndOffset(int idx, int &depth, int &offset)
Definition: binary_node.h:64
static PPolynomial BSpline(double radius=0.5)
PPolynomial< Degree-1 > derivative(void) const
PPolynomial< Degree+1 > MovingAverage(double radius)
An exception that is thrown when the arguments number or type is wrong/unhandled.
static Polynomial BSplineComponent(int i)
Definition: polynomial.hpp:310
StartingPolynomial shift(double t) const
Definition: ppolynomial.hpp:62
Polynomial< Degree > p
Definition: ppolynomial.h:46
bool RightOverlap(unsigned int, int offset)
bool LeftOverlap(unsigned int, int offset)
int ReflectLeft(unsigned int, int offset)
void SetBSplineElementIntegrals(double integrals[Degree1+1][Degree2+1])
int ReflectRight(unsigned int depth, int offset)
#define PCL_EXPORTS
Definition: pcl_macros.h:323
void differentiate(BSplineElements< Degree-1 > &d) const
void _addRight(int offset, int boundary)
void _addLeft(int offset, int boundary)
void upSample(BSplineElements &high) const