Point Cloud Library (PCL) 1.13.1
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( 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 delete[] dvDotTable ; dvDotTable = NULL;
295 delete[] ddDotTable ; ddDotTable = NULL;
296 }
297 }
298 template< int Degree , class Real >
299 void BSplineData< Degree , Real >::setSampleSpan( int idx , int& start , int& end , double smooth ) const
300 {
301 int d , off , res;
303 res = 1<<d;
304 double _start = ( off + 0.5 - 0.5*(Degree+1) ) / res - smooth;
305 double _end = ( off + 0.5 + 0.5*(Degree+1) ) / res + smooth;
306 // (start)/(sampleCount-1) >_start && (start-1)/(sampleCount-1)<=_start
307 // => start > _start * (sampleCount-1 ) && start <= _start*(sampleCount-1) + 1
308 // => _start * (sampleCount-1) + 1 >= start > _start * (sampleCount-1)
309 start = static_cast<int>( floor( _start * (sampleCount-1) + 1 ) );
310 if( start<0 ) start = 0;
311 // (end)/(sampleCount-1)<_end && (end+1)/(sampleCount-1)>=_end
312 // => end < _end * (sampleCount-1 ) && end >= _end*(sampleCount-1) - 1
313 // => _end * (sampleCount-1) > end >= _end * (sampleCount-1) - 1
314 end = static_cast<int>( ceil( _end * (sampleCount-1) - 1 ) );
315 if( end>=sampleCount ) end = sampleCount-1;
316 }
317 template<int Degree,class Real>
318 void BSplineData<Degree,Real>::setValueTables( int flags , double smooth )
319 {
320 clearValueTables();
321 if( flags & VALUE_FLAG ) valueTables = new Real[functionCount*sampleCount];
322 if( flags & D_VALUE_FLAG ) dValueTables = new Real[functionCount*sampleCount];
323 PPolynomial<Degree+1> function;
324 PPolynomial<Degree> dFunction;
325 for( int i=0 ; i<functionCount ; i++ )
326 {
327 if(smooth>0)
328 {
329 function = baseFunctions[i].MovingAverage(smooth);
330 dFunction = baseFunctions[i].derivative().MovingAverage(smooth);
331 }
332 else
333 {
334 function = baseFunctions[i];
335 dFunction = baseFunctions[i].derivative();
336 }
337 for( int j=0 ; j<sampleCount ; j++ )
338 {
339 double x=static_cast<double>(j)/(sampleCount-1);
340 if(flags & VALUE_FLAG){ valueTables[j*functionCount+i]=Real( function(x));}
341 if(flags & D_VALUE_FLAG){dValueTables[j*functionCount+i]=Real(dFunction(x));}
342 }
343 }
344 }
345 template<int Degree,class Real>
346 void BSplineData<Degree,Real>::setValueTables(int flags,double valueSmooth,double normalSmooth){
347 clearValueTables();
348 if(flags & VALUE_FLAG){ valueTables=new Real[functionCount*sampleCount];}
349 if(flags & D_VALUE_FLAG){dValueTables=new Real[functionCount*sampleCount];}
350 PPolynomial<Degree+1> function;
351 PPolynomial<Degree> dFunction;
352 for(int i=0;i<functionCount;i++){
353 if(valueSmooth>0) { function=baseFunctions[i].MovingAverage(valueSmooth);}
354 else { function=baseFunctions[i];}
355 if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);}
356 else {dFunction=baseFunctions[i].derivative();}
357
358 for(int j=0;j<sampleCount;j++){
359 double x=static_cast<double>(j)/(sampleCount-1);
360 if(flags & VALUE_FLAG){ valueTables[j*functionCount+i]=Real( function(x));}
361 if(flags & D_VALUE_FLAG){dValueTables[j*functionCount+i]=Real(dFunction(x));}
362 }
363 }
364 }
365
366
367 template<int Degree,class Real>
369 delete[] valueTables;
370 delete[] dValueTables;
371 valueTables=dValueTables=NULL;
372 }
373
374 template<int Degree,class Real>
375 inline int BSplineData<Degree,Real>::Index( int i1 , int i2 ) const { return i1*functionCount+i2; }
376 template<int Degree,class Real>
377 inline int BSplineData<Degree,Real>::SymmetricIndex( int i1 , int i2 )
378 {
379 if( i1>i2 ) return ((i1*i1+i1)>>1)+i2;
380 else return ((i2*i2+i2)>>1)+i1;
381 }
382 template<int Degree,class Real>
383 inline int BSplineData<Degree,Real>::SymmetricIndex( int i1 , int i2 , int& index )
384 {
385 if( i1<i2 )
386 {
387 index = ((i2*i2+i2)>>1)+i1;
388 return 1;
389 }
390 else
391 {
392 index = ((i1*i1+i1)>>1)+i2;
393 return 0;
394 }
395 }
396
397
398 ////////////////////////
399 // BSplineElementData //
400 ////////////////////////
401 template< int Degree >
402 BSplineElements< Degree >::BSplineElements( int res , int offset , int boundary )
403 {
404 denominator = 1;
405 this->resize( res , BSplineElementCoefficients<Degree>() );
406
407 for( int i=0 ; i<=Degree ; i++ )
408 {
409 int idx = -_off + offset + i;
410 if( idx>=0 && idx<res ) (*this)[idx][i] = 1;
411 }
412 if( boundary!=0 )
413 {
414 _addLeft( offset-2*res , boundary ) , _addRight( offset+2*res , boundary );
415 if( Degree&1 ) _addLeft( offset-res , boundary ) , _addRight( offset+res , boundary );
416 else _addLeft( -offset-1 , boundary ) , _addRight( -offset-1+2*res , boundary );
417 }
418 }
419 template< int Degree >
420 void BSplineElements< Degree >::_addLeft( int offset , int boundary )
421 {
422 int res = int( this->size() );
423 bool set = false;
424 for( int i=0 ; i<=Degree ; i++ )
425 {
426 int idx = -_off + offset + i;
427 if( idx>=0 && idx<res ) (*this)[idx][i] += boundary , set = true;
428 }
429 if( set ) _addLeft( offset-2*res , boundary );
430 }
431 template< int Degree >
432 void BSplineElements< Degree >::_addRight( int offset , int boundary )
433 {
434 int res = int( this->size() );
435 bool set = false;
436 for( int i=0 ; i<=Degree ; i++ )
437 {
438 int idx = -_off + offset + i;
439 if( idx>=0 && idx<res ) (*this)[idx][i] += boundary , set = true;
440 }
441 if( set ) _addRight( offset+2*res , boundary );
442 }
443 template< int Degree >
445 {
446 POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "B-spline up-sampling not supported for degree " << Degree);
447 }
448 template<>
450
451 template<>
453
454 template< int Degree >
456 {
457 d.resize( this->size() );
458 d.assign( d.size() , BSplineElementCoefficients< Degree-1 >() );
459 for( int i=0 ; i<int(this->size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ )
460 {
461 if( j-1>=0 ) d[i][j-1] -= (*this)[i][j];
462 if( j<Degree ) d[i][j ] += (*this)[i][j];
463 }
464 d.denominator = denominator;
465 }
466 // If we were really good, we would implement this integral table to store
467 // rational values to improve precision...
468 template< int Degree1 , int Degree2 >
469 void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] )
470 {
471 for( int i=0 ; i<=Degree1 ; i++ )
472 {
474 for( int j=0 ; j<=Degree2 ; j++ )
475 {
477 integrals[i][j] = ( p1 * p2 ).integral( 0 , 1 );
478 }
479 }
480 }
481
482
483 }
484}
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)
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