Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
bspline_data.hpp
1/*
2Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without modification,
6are permitted provided that the following conditions are met:
7
8Redistributions of source code must retain the above copyright notice, this list of
9conditions and the following disclaimer. Redistributions in binary form must reproduce
10the above copyright notice, this list of conditions and the following disclaimer
11in the documentation and/or other materials provided with the distribution.
12
13Neither the name of the Johns Hopkins University nor the names of its contributors
14may be used to endorse or promote products derived from this software without specific
15prior written permission.
16
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26DAMAGE.
27*/
28
29#include "poisson_exceptions.h"
30#include "binary_node.h"
31
32namespace 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 }
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( 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 = 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 = 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 memset( vvDotTable , 0 , sizeof(Real)*size );
184 }
185 if( flags & DV_DOT_FLAG )
186 {
187 dvDotTable = new Real[fullSize];
188 memset( dvDotTable , 0 , sizeof(Real)*fullSize );
189 }
190 if( flags & DD_DOT_FLAG )
191 {
192 ddDotTable = new Real[size];
193 memset( ddDotTable , 0 , sizeof(Real)*size );
194 }
195 double vvIntegrals[Degree+1][Degree+1];
196 double vdIntegrals[Degree+1][Degree ];
197 double dvIntegrals[Degree ][Degree+1];
198 double ddIntegrals[Degree ][Degree ];
199 int vvSums[Degree+1][Degree+1];
200 int vdSums[Degree+1][Degree ];
201 int dvSums[Degree ][Degree+1];
202 int ddSums[Degree ][Degree ];
203 SetBSplineElementIntegrals< Degree , Degree >( vvIntegrals );
204 SetBSplineElementIntegrals< Degree , Degree-1 >( vdIntegrals );
205 SetBSplineElementIntegrals< Degree-1 , Degree >( dvIntegrals );
206 SetBSplineElementIntegrals< Degree-1 , Degree-1 >( ddIntegrals );
207
208 for( int d1=0 ; d1<=depth ; d1++ )
209 for( int off1=0 ; off1<(1<<d1) ; off1++ )
210 {
211 int ii = BinaryNode< Real >::CenterIndex( d1 , off1 );
213 BSplineElements< Degree-1 > db1;
214 b1.differentiate( db1 );
215
216 int start1 , end1;
217
218 start1 = -1;
219 for( int i=0 ; i<int(b1.size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
220 {
221 if( b1[i][j] && start1==-1 ) start1 = i;
222 if( b1[i][j] ) end1 = i+1;
223 }
224 for( int d2=d1 ; d2<=depth ; d2++ )
225 {
226 for( int off2=0 ; off2<(1<<d2) ; off2++ )
227 {
228 int start2 = off2-Degree;
229 int end2 = off2+Degree+1;
230 if( start2>=end1 || start1>=end2 ) continue;
231 start2 = std::max< int >( start1 , start2 );
232 end2 = std::min< int >( end1 , end2 );
233 if( d1==d2 && off2<off1 ) continue;
234 int jj = BinaryNode< Real >::CenterIndex( d2 , off2 );
236 BSplineElements< Degree-1 > db2;
237 b2.differentiate( db2 );
238
239 int idx = SymmetricIndex( ii , jj );
240 int idx1 = Index( ii , jj ) , idx2 = Index( jj , ii );
241
242 memset( vvSums , 0 , sizeof( int ) * ( Degree+1 ) * ( Degree+1 ) );
243 memset( vdSums , 0 , sizeof( int ) * ( Degree+1 ) * ( Degree ) );
244 memset( dvSums , 0 , sizeof( int ) * ( Degree ) * ( Degree+1 ) );
245 memset( ddSums , 0 , sizeof( int ) * ( Degree ) * ( Degree ) );
246 for( int i=start2 ; i<end2 ; i++ )
247 {
248 for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) vvSums[j][k] += b1[i][j] * b2[i][k];
249 for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) vdSums[j][k] += b1[i][j] * db2[i][k];
250 for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) dvSums[j][k] += db1[i][j] * b2[i][k];
251 for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) ddSums[j][k] += db1[i][j] * db2[i][k];
252 }
253 double vvDot = 0 , dvDot = 0 , vdDot = 0 , ddDot = 0;
254 for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) vvDot += vvIntegrals[j][k] * vvSums[j][k];
255 for( int j=0 ; j<=Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) vdDot += vdIntegrals[j][k] * vdSums[j][k];
256 for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k<=Degree ; k++ ) dvDot += dvIntegrals[j][k] * dvSums[j][k];
257 for( int j=0 ; j< Degree ; j++ ) for( int k=0 ; k< Degree ; k++ ) ddDot += ddIntegrals[j][k] * ddSums[j][k];
258 vvDot /= (1<<d2);
259 ddDot *= (1<<d2);
260 vvDot /= ( b1.denominator * b2.denominator );
261 dvDot /= ( b1.denominator * b2.denominator );
262 vdDot /= ( b1.denominator * b2.denominator );
263 ddDot /= ( b1.denominator * b2.denominator );
264 if( fabs(vvDot)<1e-15 ) continue;
265 if( flags & VV_DOT_FLAG ) vvDotTable [idx] = Real( vvDot );
266 if( useDotRatios )
267 {
268 if( flags & DV_DOT_FLAG ) dvDotTable[idx1] = Real( dvDot / vvDot );
269 if( flags & DV_DOT_FLAG ) dvDotTable[idx2] = Real( vdDot / vvDot );
270 if( flags & DD_DOT_FLAG ) ddDotTable[idx ] = Real( ddDot / vvDot );
271 }
272 else
273 {
274 if( flags & DV_DOT_FLAG ) dvDotTable[idx1] = Real( dvDot );
275 if( flags & DV_DOT_FLAG ) dvDotTable[idx2] = Real( dvDot );
276 if( flags & DD_DOT_FLAG ) ddDotTable[idx ] = Real( ddDot );
277 }
278 }
280 b = b1;
281 b.upSample( b1 );
282 b1.differentiate( db1 );
283 start1 = -1;
284 for( int i=0 ; i<int(b1.size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
285 {
286 if( b1[i][j] && start1==-1 ) start1 = i;
287 if( b1[i][j] ) end1 = i+1;
288 }
289 }
290 }
291 }
292 template<int Degree,class Real>
294 {
295 if (flags & VV_DOT_FLAG) {
296 delete[] vvDotTable ; vvDotTable = NULL;
297 delete[] dvDotTable ; dvDotTable = NULL;
298 delete[] ddDotTable ; ddDotTable = NULL;
299 }
300 }
301 template< int Degree , class Real >
302 void BSplineData< Degree , Real >::setSampleSpan( int idx , int& start , int& end , double smooth ) const
303 {
304 int d , off , res;
306 res = 1<<d;
307 double _start = ( off + 0.5 - 0.5*(Degree+1) ) / res - smooth;
308 double _end = ( off + 0.5 + 0.5*(Degree+1) ) / res + smooth;
309 // (start)/(sampleCount-1) >_start && (start-1)/(sampleCount-1)<=_start
310 // => start > _start * (sampleCount-1 ) && start <= _start*(sampleCount-1) + 1
311 // => _start * (sampleCount-1) + 1 >= start > _start * (sampleCount-1)
312 start = int( floor( _start * (sampleCount-1) + 1 ) );
313 if( start<0 ) start = 0;
314 // (end)/(sampleCount-1)<_end && (end+1)/(sampleCount-1)>=_end
315 // => end < _end * (sampleCount-1 ) && end >= _end*(sampleCount-1) - 1
316 // => _end * (sampleCount-1) > end >= _end * (sampleCount-1) - 1
317 end = int( ceil( _end * (sampleCount-1) - 1 ) );
318 if( end>=sampleCount ) end = sampleCount-1;
319 }
320 template<int Degree,class Real>
321 void BSplineData<Degree,Real>::setValueTables( int flags , double smooth )
322 {
323 clearValueTables();
324 if( flags & VALUE_FLAG ) valueTables = new Real[functionCount*sampleCount];
325 if( flags & D_VALUE_FLAG ) dValueTables = new Real[functionCount*sampleCount];
326 PPolynomial<Degree+1> function;
327 PPolynomial<Degree> dFunction;
328 for( int i=0 ; i<functionCount ; i++ )
329 {
330 if(smooth>0)
331 {
332 function = baseFunctions[i].MovingAverage(smooth);
333 dFunction = baseFunctions[i].derivative().MovingAverage(smooth);
334 }
335 else
336 {
337 function = baseFunctions[i];
338 dFunction = baseFunctions[i].derivative();
339 }
340 for( int j=0 ; j<sampleCount ; j++ )
341 {
342 double x=double(j)/(sampleCount-1);
343 if(flags & VALUE_FLAG){ valueTables[j*functionCount+i]=Real( function(x));}
344 if(flags & D_VALUE_FLAG){dValueTables[j*functionCount+i]=Real(dFunction(x));}
345 }
346 }
347 }
348 template<int Degree,class Real>
349 void BSplineData<Degree,Real>::setValueTables(int flags,double valueSmooth,double normalSmooth){
350 clearValueTables();
351 if(flags & VALUE_FLAG){ valueTables=new Real[functionCount*sampleCount];}
352 if(flags & D_VALUE_FLAG){dValueTables=new Real[functionCount*sampleCount];}
353 PPolynomial<Degree+1> function;
354 PPolynomial<Degree> dFunction;
355 for(int i=0;i<functionCount;i++){
356 if(valueSmooth>0) { function=baseFunctions[i].MovingAverage(valueSmooth);}
357 else { function=baseFunctions[i];}
358 if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);}
359 else {dFunction=baseFunctions[i].derivative();}
360
361 for(int j=0;j<sampleCount;j++){
362 double x=double(j)/(sampleCount-1);
363 if(flags & VALUE_FLAG){ valueTables[j*functionCount+i]=Real( function(x));}
364 if(flags & D_VALUE_FLAG){dValueTables[j*functionCount+i]=Real(dFunction(x));}
365 }
366 }
367 }
368
369
370 template<int Degree,class Real>
372 delete[] valueTables;
373 delete[] dValueTables;
374 valueTables=dValueTables=NULL;
375 }
376
377 template<int Degree,class Real>
378 inline int BSplineData<Degree,Real>::Index( int i1 , int i2 ) const { return i1*functionCount+i2; }
379 template<int Degree,class Real>
380 inline int BSplineData<Degree,Real>::SymmetricIndex( int i1 , int i2 )
381 {
382 if( i1>i2 ) return ((i1*i1+i1)>>1)+i2;
383 else return ((i2*i2+i2)>>1)+i1;
384 }
385 template<int Degree,class Real>
386 inline int BSplineData<Degree,Real>::SymmetricIndex( int i1 , int i2 , int& index )
387 {
388 if( i1<i2 )
389 {
390 index = ((i2*i2+i2)>>1)+i1;
391 return 1;
392 }
393 else
394 {
395 index = ((i1*i1+i1)>>1)+i2;
396 return 0;
397 }
398 }
399
400
401 ////////////////////////
402 // BSplineElementData //
403 ////////////////////////
404 template< int Degree >
405 BSplineElements< Degree >::BSplineElements( int res , int offset , int boundary )
406 {
407 denominator = 1;
408 this->resize( res , BSplineElementCoefficients<Degree>() );
409
410 for( int i=0 ; i<=Degree ; i++ )
411 {
412 int idx = -_off + offset + i;
413 if( idx>=0 && idx<res ) (*this)[idx][i] = 1;
414 }
415 if( boundary!=0 )
416 {
417 _addLeft( offset-2*res , boundary ) , _addRight( offset+2*res , boundary );
418 if( Degree&1 ) _addLeft( offset-res , boundary ) , _addRight( offset+res , boundary );
419 else _addLeft( -offset-1 , boundary ) , _addRight( -offset-1+2*res , boundary );
420 }
421 }
422 template< int Degree >
423 void BSplineElements< Degree >::_addLeft( int offset , int boundary )
424 {
425 int res = int( this->size() );
426 bool set = false;
427 for( int i=0 ; i<=Degree ; i++ )
428 {
429 int idx = -_off + offset + i;
430 if( idx>=0 && idx<res ) (*this)[idx][i] += boundary , set = true;
431 }
432 if( set ) _addLeft( offset-2*res , boundary );
433 }
434 template< int Degree >
435 void BSplineElements< Degree >::_addRight( int offset , int boundary )
436 {
437 int res = int( this->size() );
438 bool set = false;
439 for( int i=0 ; i<=Degree ; i++ )
440 {
441 int idx = -_off + offset + i;
442 if( idx>=0 && idx<res ) (*this)[idx][i] += boundary , set = true;
443 }
444 if( set ) _addRight( offset+2*res , boundary );
445 }
446 template< int Degree >
448 {
449 POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "B-spline up-sampling not supported for degree " << Degree);
450 }
451 template<>
453
454 template<>
456
457 template< int Degree >
459 {
460 d.resize( this->size() );
461 d.assign( d.size() , BSplineElementCoefficients< Degree-1 >() );
462 for( int i=0 ; i<int(this->size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
463 {
464 if( j-1>=0 ) d[i][j-1] -= (*this)[i][j];
465 if( j<Degree ) d[i][j ] += (*this)[i][j];
466 }
467 d.denominator = denominator;
468 }
469 // If we were really good, we would implement this integral table to store
470 // rational values to improve precision...
471 template< int Degree1 , int Degree2 >
472 void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] )
473 {
474 for( int i=0 ; i<=Degree1 ; i++ )
475 {
477 for( int j=0 ; j<=Degree2 ; j++ )
478 {
480 integrals[i][j] = ( p1 * p2 ).integral( 0 , 1 );
481 }
482 }
483 }
484
485
486 }
487}
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 setDotTables(int flags)
virtual void clearValueTables(void)
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)
StartingPolynomial shift(double t) const
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)
void differentiate(BSplineElements< Degree-1 > &d) const
void _addRight(int offset, int boundary)
void _addLeft(int offset, int boundary)
void upSample(BSplineElements &high) const