Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 ////////////////// 00030 // FunctionData // 00031 ////////////////// 00032 00033 namespace pcl 00034 { 00035 namespace poisson 00036 { 00037 00038 template<int Degree,class Real> 00039 FunctionData<Degree,Real>::FunctionData(void) 00040 { 00041 dotTable=dDotTable=d2DotTable=NULL; 00042 valueTables=dValueTables=NULL; 00043 res=0; 00044 } 00045 00046 template<int Degree,class Real> 00047 FunctionData<Degree,Real>::~FunctionData(void) 00048 { 00049 if(res) 00050 { 00051 if( dotTable) delete[] dotTable; 00052 if( dDotTable) delete[] dDotTable; 00053 if(d2DotTable) delete[] d2DotTable; 00054 if( valueTables) delete[] valueTables; 00055 if(dValueTables) delete[] dValueTables; 00056 } 00057 dotTable=dDotTable=d2DotTable=NULL; 00058 valueTables=dValueTables=NULL; 00059 res=0; 00060 } 00061 00062 template<int Degree,class Real> 00063 #if BOUNDARY_CONDITIONS 00064 void FunctionData<Degree,Real>::set( const int& maxDepth , const PPolynomial<Degree>& F , const int& normalize , bool useDotRatios , bool reflectBoundary ) 00065 #else // !BOUNDARY_CONDITIONS 00066 void FunctionData<Degree,Real>::set(const int& maxDepth,const PPolynomial<Degree>& F,const int& normalize , bool useDotRatios ) 00067 #endif // BOUNDARY_CONDITIONS 00068 { 00069 this->normalize = normalize; 00070 this->useDotRatios = useDotRatios; 00071 #if BOUNDARY_CONDITIONS 00072 this->reflectBoundary = reflectBoundary; 00073 #endif // BOUNDARY_CONDITIONS 00074 00075 depth = maxDepth; 00076 res = BinaryNode<double>::CumulativeCenterCount( depth ); 00077 res2 = (1<<(depth+1))+1; 00078 baseFunctions = new PPolynomial<Degree+1>[res]; 00079 // Scale the function so that it has: 00080 // 0] Value 1 at 0 00081 // 1] Integral equal to 1 00082 // 2] Square integral equal to 1 00083 switch( normalize ) 00084 { 00085 case 2: 00086 baseFunction=F/sqrt((F*F).integral(F.polys[0].start,F.polys[F.polyCount-1].start)); 00087 break; 00088 case 1: 00089 baseFunction=F/F.integral(F.polys[0].start,F.polys[F.polyCount-1].start); 00090 break; 00091 default: 00092 baseFunction=F/F(0); 00093 } 00094 dBaseFunction = baseFunction.derivative(); 00095 #if BOUNDARY_CONDITIONS 00096 leftBaseFunction = baseFunction + baseFunction.shift( -1 ); 00097 rightBaseFunction = baseFunction + baseFunction.shift( 1 ); 00098 dLeftBaseFunction = leftBaseFunction.derivative(); 00099 dRightBaseFunction = rightBaseFunction.derivative(); 00100 #endif // BOUNDARY_CONDITIONS 00101 double c1,w1; 00102 for( int i=0 ; i<res ; i++ ) 00103 { 00104 BinaryNode< double >::CenterAndWidth( i , c1 , w1 ); 00105 #if BOUNDARY_CONDITIONS 00106 if( reflectBoundary ) 00107 { 00108 int d , off; 00109 BinaryNode< double >::DepthAndOffset( i , d , off ); 00110 if ( off==0 ) baseFunctions[i] = leftBaseFunction.scale( w1 ).shift( c1 ); 00111 else if( off==((1<<d)-1) ) baseFunctions[i] = rightBaseFunction.scale( w1 ).shift( c1 ); 00112 else baseFunctions[i] = baseFunction.scale( w1 ).shift( c1 ); 00113 } 00114 else baseFunctions[i] = baseFunction.scale(w1).shift(c1); 00115 #else // !BOUNDARY_CONDITIONS 00116 baseFunctions[i] = baseFunction.scale(w1).shift(c1); 00117 #endif // BOUNDARY_CONDITIONS 00118 // Scale the function so that it has L2-norm equal to one 00119 switch( normalize ) 00120 { 00121 case 2: 00122 baseFunctions[i]/=sqrt(w1); 00123 break; 00124 case 1: 00125 baseFunctions[i]/=w1; 00126 break; 00127 } 00128 } 00129 } 00130 template<int Degree,class Real> 00131 void FunctionData<Degree,Real>::setDotTables( const int& flags ) 00132 { 00133 clearDotTables( flags ); 00134 int size; 00135 size = ( res*res + res )>>1; 00136 if( flags & DOT_FLAG ) 00137 { 00138 dotTable = new Real[size]; 00139 memset( dotTable , 0 , sizeof(Real)*size ); 00140 } 00141 if( flags & D_DOT_FLAG ) 00142 { 00143 dDotTable = new Real[size]; 00144 memset( dDotTable , 0 , sizeof(Real)*size ); 00145 } 00146 if( flags & D2_DOT_FLAG ) 00147 { 00148 d2DotTable = new Real[size]; 00149 memset( d2DotTable , 0 , sizeof(Real)*size ); 00150 } 00151 double t1 , t2; 00152 t1 = baseFunction.polys[0].start; 00153 t2 = baseFunction.polys[baseFunction.polyCount-1].start; 00154 for( int i=0 ; i<res ; i++ ) 00155 { 00156 double c1 , c2 , w1 , w2; 00157 BinaryNode<double>::CenterAndWidth( i , c1 , w1 ); 00158 #if BOUNDARY_CONDITIONS 00159 int d1 , d2 , off1 , off2; 00160 BinaryNode< double >::DepthAndOffset( i , d1 , off1 ); 00161 int boundary1 = 0; 00162 if ( reflectBoundary && off1==0 ) boundary1 = -1; 00163 else if( reflectBoundary && off1==( (1<<d1)-1 ) ) boundary1 = 1; 00164 #endif // BOUNDARY_CONDITIONS 00165 double start1 = t1 * w1 + c1; 00166 double end1 = t2 * w1 + c1; 00167 for( int j=0 ; j<=i ; j++ ) 00168 { 00169 BinaryNode<double>::CenterAndWidth( j , c2 , w2 ); 00170 #if BOUNDARY_CONDITIONS 00171 BinaryNode< double >::DepthAndOffset( j , d2 , off2 ); 00172 int boundary2 = 0; 00173 if ( reflectBoundary && off2==0 ) boundary2 = -1; 00174 else if( reflectBoundary && off2==( (1<<d2)-1 ) ) boundary2 = 1; 00175 #endif // BOUNDARY_CONDITIONS 00176 int idx = SymmetricIndex( i , j ); 00177 00178 double start = t1 * w2 + c2; 00179 double end = t2 * w2 + c2; 00180 #if BOUNDARY_CONDITIONS 00181 if( reflectBoundary ) 00182 { 00183 if( start<0 ) start = 0; 00184 if( start>1 ) start = 1; 00185 if( end <0 ) end = 0; 00186 if( end >1 ) end = 1; 00187 } 00188 #endif // BOUNDARY_CONDITIONS 00189 00190 if( start< start1 ) start = start1; 00191 if( end > end1 ) end = end1; 00192 if( start>= end ) continue; 00193 00194 #if BOUNDARY_CONDITIONS 00195 Real dot = dotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); 00196 #else // !BOUNDARY_CONDITIONS 00197 Real dot = dotProduct( c1 , w1 , c2 , w2 ); 00198 #endif // BOUNDARY_CONDITIONS 00199 if( fabs(dot)<1e-15 ) continue; 00200 if( flags & DOT_FLAG ) dotTable[idx]=dot; 00201 if( useDotRatios ) 00202 { 00203 #if BOUNDARY_CONDITIONS 00204 if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot; 00205 if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot; 00206 #else // !BOUNDARY_CONDITIONS 00207 if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct(c1,w1,c2,w2)/dot; 00208 if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2)/dot; 00209 #endif // BOUNDARY_CONDITIONS 00210 } 00211 else 00212 { 00213 #if BOUNDARY_CONDITIONS 00214 if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); 00215 if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); 00216 #else // !BOUNDARY_CONDTIONS 00217 if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct(c1,w1,c2,w2); 00218 if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2); 00219 #endif // BOUNDARY_CONDITIONS 00220 } 00221 } 00222 } 00223 } 00224 template<int Degree,class Real> 00225 void FunctionData<Degree,Real>::clearDotTables( const int& flags ) 00226 { 00227 if((flags & DOT_FLAG) && dotTable) 00228 { 00229 delete[] dotTable; 00230 dotTable=NULL; 00231 } 00232 if((flags & D_DOT_FLAG) && dDotTable) 00233 { 00234 delete[] dDotTable; 00235 dDotTable=NULL; 00236 } 00237 if((flags & D2_DOT_FLAG) && d2DotTable) 00238 { 00239 delete[] d2DotTable; 00240 d2DotTable=NULL; 00241 } 00242 } 00243 template<int Degree,class Real> 00244 void FunctionData<Degree,Real>::setValueTables( const int& flags , const double& smooth ) 00245 { 00246 clearValueTables(); 00247 if( flags & VALUE_FLAG ) valueTables = new Real[res*res2]; 00248 if( flags & D_VALUE_FLAG ) dValueTables = new Real[res*res2]; 00249 PPolynomial<Degree+1> function; 00250 PPolynomial<Degree> dFunction; 00251 for( int i=0 ; i<res ; i++ ) 00252 { 00253 if(smooth>0) 00254 { 00255 function=baseFunctions[i].MovingAverage(smooth); 00256 dFunction=baseFunctions[i].derivative().MovingAverage(smooth); 00257 } 00258 else 00259 { 00260 function=baseFunctions[i]; 00261 dFunction=baseFunctions[i].derivative(); 00262 } 00263 for( int j=0 ; j<res2 ; j++ ) 00264 { 00265 double x=double(j)/(res2-1); 00266 if(flags & VALUE_FLAG){ valueTables[j*res+i]=Real( function(x));} 00267 if(flags & D_VALUE_FLAG){dValueTables[j*res+i]=Real(dFunction(x));} 00268 } 00269 } 00270 } 00271 template<int Degree,class Real> 00272 void FunctionData<Degree,Real>::setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth){ 00273 clearValueTables(); 00274 if(flags & VALUE_FLAG){ valueTables=new Real[res*res2];} 00275 if(flags & D_VALUE_FLAG){dValueTables=new Real[res*res2];} 00276 PPolynomial<Degree+1> function; 00277 PPolynomial<Degree> dFunction; 00278 for(int i=0;i<res;i++){ 00279 if(valueSmooth>0) { function=baseFunctions[i].MovingAverage(valueSmooth);} 00280 else { function=baseFunctions[i];} 00281 if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);} 00282 else {dFunction=baseFunctions[i].derivative();} 00283 00284 for(int j=0;j<res2;j++){ 00285 double x=double(j)/(res2-1); 00286 if(flags & VALUE_FLAG){ valueTables[j*res+i]=Real( function(x));} 00287 if(flags & D_VALUE_FLAG){dValueTables[j*res+i]=Real(dFunction(x));} 00288 } 00289 } 00290 } 00291 00292 00293 template<int Degree,class Real> 00294 void FunctionData<Degree,Real>::clearValueTables(void){ 00295 if( valueTables){delete[] valueTables;} 00296 if(dValueTables){delete[] dValueTables;} 00297 valueTables=dValueTables=NULL; 00298 } 00299 00300 #if BOUNDARY_CONDITIONS 00301 template<int Degree,class Real> 00302 Real FunctionData<Degree,Real>::dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const 00303 { 00304 const PPolynomial< Degree > *b1 , *b2; 00305 if ( boundary1==-1 ) b1 = & leftBaseFunction; 00306 else if( boundary1== 0 ) b1 = & baseFunction; 00307 else if( boundary1== 1 ) b1 = &rightBaseFunction; 00308 if ( boundary2==-1 ) b2 = & leftBaseFunction; 00309 else if( boundary2== 0 ) b2 = & baseFunction; 00310 else if( boundary2== 1 ) b2 = &rightBaseFunction; 00311 double r=fabs( baseFunction.polys[0].start ); 00312 switch( normalize ) 00313 { 00314 case 2: 00315 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2)); 00316 case 1: 00317 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2)); 00318 default: 00319 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1); 00320 } 00321 } 00322 template<int Degree,class Real> 00323 Real FunctionData<Degree,Real>::dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const 00324 { 00325 const PPolynomial< Degree-1 > *b1; 00326 const PPolynomial< Degree > *b2; 00327 if ( boundary1==-1 ) b1 = & dLeftBaseFunction; 00328 else if( boundary1== 0 ) b1 = & dBaseFunction; 00329 else if( boundary1== 1 ) b1 = &dRightBaseFunction; 00330 if ( boundary2==-1 ) b2 = & leftBaseFunction; 00331 else if( boundary2== 0 ) b2 = & baseFunction; 00332 else if( boundary2== 1 ) b2 = & rightBaseFunction; 00333 double r=fabs(baseFunction.polys[0].start); 00334 switch(normalize){ 00335 case 2: 00336 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2)); 00337 case 1: 00338 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2)); 00339 default: 00340 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)); 00341 } 00342 } 00343 template<int Degree,class Real> 00344 Real FunctionData<Degree,Real>::d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const 00345 { 00346 const PPolynomial< Degree-1 > *b1 , *b2; 00347 if ( boundary1==-1 ) b1 = & dLeftBaseFunction; 00348 else if( boundary1== 0 ) b1 = & dBaseFunction; 00349 else if( boundary1== 1 ) b1 = &dRightBaseFunction; 00350 if ( boundary2==-1 ) b2 = & dLeftBaseFunction; 00351 else if( boundary2== 0 ) b2 = & dBaseFunction; 00352 else if( boundary2== 1 ) b2 = &dRightBaseFunction; 00353 double r=fabs(baseFunction.polys[0].start); 00354 switch( normalize ) 00355 { 00356 case 2: 00357 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2)); 00358 case 1: 00359 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2)); 00360 default: 00361 return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2); 00362 } 00363 } 00364 #else // !BOUNDARY_CONDITIONS 00365 template<int Degree,class Real> 00366 Real FunctionData<Degree,Real>::dotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ 00367 double r=fabs(baseFunction.polys[0].start); 00368 switch( normalize ) 00369 { 00370 case 2: 00371 return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2)); 00372 case 1: 00373 return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2)); 00374 default: 00375 return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1); 00376 } 00377 } 00378 template<int Degree,class Real> 00379 Real FunctionData<Degree,Real>::dDotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ 00380 double r=fabs(baseFunction.polys[0].start); 00381 switch(normalize){ 00382 case 2: 00383 return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2)); 00384 case 1: 00385 return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2)); 00386 default: 00387 return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)); 00388 } 00389 } 00390 template<int Degree,class Real> 00391 Real FunctionData<Degree,Real>::d2DotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ 00392 double r=fabs(baseFunction.polys[0].start); 00393 switch(normalize){ 00394 case 2: 00395 return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2)); 00396 case 1: 00397 return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2)); 00398 default: 00399 return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2); 00400 } 00401 } 00402 #endif // BOUNDARY_CONDITIONS 00403 template<int Degree,class Real> 00404 inline int FunctionData<Degree,Real>::SymmetricIndex( const int& i1 , const int& i2 ) 00405 { 00406 if( i1>i2 ) return ((i1*i1+i1)>>1)+i2; 00407 else return ((i2*i2+i2)>>1)+i1; 00408 } 00409 template<int Degree,class Real> 00410 inline int FunctionData<Degree,Real>::SymmetricIndex( const int& i1 , const int& i2 , int& index ) 00411 { 00412 if( i1<i2 ) 00413 { 00414 index = ((i2*i2+i2)>>1)+i1; 00415 return 1; 00416 } 00417 else{ 00418 index = ((i1*i1+i1)>>1)+i2; 00419 return 0; 00420 } 00421 } 00422 } 00423 }