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


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:13