function_data.hpp
Go to the documentation of this file.
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 
00030 // FunctionData //
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 }

pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:19