Point Cloud Library (PCL) 1.14.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( 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;
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)
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