bivariate_polynomial.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) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: bivariate_polynomial.hpp 4968 2012-03-08 06:39:52Z rusu $
00037  *
00038  */
00039 #ifndef BIVARIATE_POLYNOMIAL_HPP
00040 #define BIVARIATE_POLYNOMIAL_HPP
00041 
00043 template<typename real>
00044 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree) :
00045   degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
00046 {
00047   setDegree(new_degree);
00048 }
00049 
00051 template<typename real>
00052 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) :
00053   degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
00054 {
00055   deepCopy (other);
00056 }
00057 
00059 template<typename real>
00060 pcl::BivariatePolynomialT<real>::~BivariatePolynomialT ()
00061 {
00062   memoryCleanUp ();
00063 }
00064 
00066 template<typename real> void
00067 pcl::BivariatePolynomialT<real>::setDegree (int newDegree)
00068 {
00069   if (newDegree <= 0)
00070   {
00071     degree = -1;
00072     memoryCleanUp();
00073     return;
00074   }
00075   int oldDegree = degree;
00076   degree = newDegree;
00077   if (oldDegree != degree)
00078   {
00079     delete[] parameters;
00080     parameters = new real[getNoOfParameters ()];
00081   }
00082   delete gradient_x; gradient_x = NULL;
00083   delete gradient_y; gradient_y = NULL;
00084 }
00085 
00087 template<typename real> void
00088 pcl::BivariatePolynomialT<real>::memoryCleanUp ()
00089 {
00090   delete[] parameters; parameters = NULL;
00091   delete gradient_x; gradient_x = NULL;
00092   delete gradient_y; gradient_y = NULL;
00093 }
00094 
00096 template<typename real> void
00097 pcl::BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other)
00098 {
00099   if (this == &other) return;
00100   if (degree != other.degree) 
00101   {
00102     memoryCleanUp ();
00103     degree = other.degree;
00104     parameters = new real[getNoOfParameters ()];
00105   }
00106   if (other.gradient_x == NULL) 
00107   {
00108     delete gradient_x; gradient_x=NULL;
00109     delete gradient_y; gradient_y=NULL;
00110   }
00111   else if (gradient_x==NULL) 
00112   {
00113     gradient_x = new pcl::BivariatePolynomialT<real> ();
00114     gradient_y = new pcl::BivariatePolynomialT<real> ();
00115   }
00116   real* tmpParameters1 = parameters;
00117   const real* tmpParameters2 = other.parameters;
00118   unsigned int noOfParameters = getNoOfParameters ();
00119   for (unsigned int i=0; i<noOfParameters; i++)
00120     *tmpParameters1++ = *tmpParameters2++;
00121 
00122   if (other.gradient_x != NULL) 
00123   {
00124     gradient_x->deepCopy (*other.gradient_x);
00125     gradient_y->deepCopy (*other.gradient_y);
00126   }
00127 }
00128 
00130 template<typename real> void
00131 pcl::BivariatePolynomialT<real>::calculateGradient (bool forceRecalc)
00132 {
00133   if (gradient_x!=NULL && !forceRecalc) 
00134   
00135   if (gradient_x == NULL)
00136     gradient_x = new pcl::BivariatePolynomialT<real> (degree-1);
00137   if (gradient_y == NULL)
00138     gradient_y = new pcl::BivariatePolynomialT<real> (degree-1);
00139   
00140   unsigned int parameterPosDx=0, parameterPosDy=0;
00141   for (int xDegree=degree; xDegree>=0; xDegree--) 
00142   {
00143     for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) 
00144     {
00145       if (xDegree > 0) 
00146       {
00147         gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
00148         parameterPosDx++;
00149       }
00150       if (yDegree > 0) 
00151       {
00152         gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
00153                                                                         yDegree - 1];
00154         parameterPosDy++;
00155       }
00156     }
00157   }
00158 }
00159 
00161 template<typename real> real
00162 pcl::BivariatePolynomialT<real>::getValue (real x, real y) const
00163 {
00164   unsigned int parametersSize = getNoOfParameters ();
00165   real* tmpParameter = &parameters[parametersSize-1];
00166   real tmpX=1.0, tmpY, ret=0;
00167   for (int xDegree=0; xDegree<=degree; xDegree++) 
00168   {
00169     tmpY = 1.0;
00170     for (int yDegree=0; yDegree<=degree-xDegree; yDegree++)
00171     {
00172       ret += (*tmpParameter)*tmpX*tmpY;
00173       tmpY *= y;
00174       tmpParameter--;
00175     }
00176     tmpX *= x;
00177   }
00178   return ret;
00179 }
00180 
00182 template<typename real> void
00183 pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
00184 {
00185   calculateGradient ();
00186   gradX = gradient_x->getValue (x, y);
00187   gradY = gradient_y->getValue (x, y);
00188 }
00189 
00191 template<typename real> void
00192 pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
00193                                                      std::vector<int>& types) const
00194 {
00195   x_values.clear ();
00196   y_values.clear ();
00197   types.clear ();
00198   
00199   if (degree == 2)
00200   {
00201     real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
00202              (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
00203          y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
00204     
00205     if (!pcl_isfinite(x) || !pcl_isfinite(y))
00206       return;
00207     
00208     int type = 2;
00209     real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
00210     //std::cout << "det(H) = "<<det_H<<"\n";
00211     if (det_H > real(0))  // Check Hessian determinant
00212     {
00213       if (parameters[0]+parameters[3] < real(0))  // Check Hessian trace
00214         type = 0;
00215       else
00216         type = 1;
00217     }
00218     x_values.push_back(x);
00219     y_values.push_back(y);
00220     types.push_back(type);
00221   }
00222   else
00223   {
00224     std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
00225   }
00226 }
00227 
00229 template<typename real> std::ostream&
00230 pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
00231 {
00232   real* tmpParameter = p.parameters;
00233   bool first = true;
00234   real currentParameter;
00235   for (int xDegree=p.degree; xDegree>=0; xDegree--) 
00236   {
00237     for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) 
00238     {
00239       currentParameter = *tmpParameter;
00240       if (!first) 
00241       {
00242         os << (currentParameter<0.0?" - ":" + ");
00243         currentParameter = fabs (currentParameter);
00244       }
00245       os << currentParameter;
00246       if (xDegree>0) 
00247       {
00248         os << "x";
00249         if (xDegree>1)
00250           os<<"^"<<xDegree;
00251       }
00252       if (yDegree>0) 
00253       {
00254         os << "y";
00255         if (yDegree>1)
00256           os<<"^"<<yDegree;
00257       }
00258       
00259       first = false;
00260       tmpParameter++;
00261     }
00262   }
00263   return (os);
00264 }
00265 
00267 template<typename real> void
00268 pcl::BivariatePolynomialT<real>::writeBinary (std::ostream& os) const
00269 {
00270   os.write (reinterpret_cast<char*> (&degree), sizeof (int));
00271   unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
00272   os.write (reinterpret_cast<char*> (this->parameters), paramCnt * sizeof (real));
00273 }
00274 
00276 template<typename real> void
00277 pcl::BivariatePolynomialT<real>::writeBinary (const char* filename) const
00278 {
00279   std::ofstream fout (filename);
00280   writeBinary (fout);
00281 }
00282 
00284 template<typename real> void
00285 pcl::BivariatePolynomialT<real>::readBinary (std::istream& os)
00286 {
00287   memoryCleanUp ();
00288   os.read (reinterpret_cast<char*> (&this->degree), sizeof (int));
00289   unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
00290   parameters = new real[paramCnt];
00291   os.read (reinterpret_cast<char*> (&(*this->parameters)), paramCnt * sizeof (real));
00292 }
00293 
00295 template<typename real> void
00296 pcl::BivariatePolynomialT<real>::readBinary (const char* filename)
00297 {
00298   std::ifstream fin (filename);
00299   readBinary (fin);
00300 }
00301 
00302 #endif
00303 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:40