Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
function_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//////////////////
30// FunctionData //
31//////////////////
32
33namespace pcl
34{
35 namespace poisson
36 {
37
38 template<int Degree,class Real>
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::D_VALUE_FLAG
static const int D_VALUE_FLAG
Definition function_data.h:58
pcl::poisson::FunctionData::dotProduct
Real dotProduct(const double &center1, const double &width1, const double &center2, const double &width2) const
Definition function_data.hpp:360
pcl::poisson::FunctionData::baseFunction
PPolynomial< Degree > baseFunction
Definition function_data.h:67
pcl::poisson::FunctionData::d2DotProduct
Real d2DotProduct(const double &center1, const double &width1, const double &center2, const double &width2) const
Definition function_data.hpp:385
pcl::poisson::FunctionData::DOT_FLAG
static const int DOT_FLAG
Definition function_data.h:54
pcl::poisson::FunctionData::D_DOT_FLAG
static const int D_DOT_FLAG
Definition function_data.h:55
pcl::poisson::FunctionData::dBaseFunction
PPolynomial< Degree-1 > dBaseFunction
Definition function_data.h:68
pcl::poisson::FunctionData::clearValueTables
virtual void clearValueTables(void)
Definition function_data.hpp:288
pcl::poisson::FunctionData::baseFunctions
PPolynomial< Degree+1 > * baseFunctions
Definition function_data.h:70
pcl::poisson::FunctionData::VALUE_FLAG
static const int VALUE_FLAG
Definition function_data.h:57
pcl::poisson::FunctionData::D2_DOT_FLAG
static const int D2_DOT_FLAG
Definition function_data.h:56
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)
Definition function_data.hpp:66
pcl::poisson::FunctionData::clearDotTables
virtual void clearDotTables(const int &flags)
Definition function_data.hpp:225
pcl::poisson::FunctionData::dDotProduct
Real dDotProduct(const double &center1, const double &width1, const double &center2, const double &width2) const
Definition function_data.hpp:373
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