Point Cloud Library (PCL)  1.3.1
bivariate_polynomial.hpp
Go to the documentation of this file.
00001 template<typename real>
00002 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree) :
00003   degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
00004 {
00005   setDegree(new_degree);
00006 }
00007 
00008 template<typename real>
00009 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) :
00010   degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
00011 {
00012   deepCopy (other);
00013 }
00014 
00015 template<typename real>
00016 pcl::BivariatePolynomialT<real>::~BivariatePolynomialT ()
00017 {
00018   memoryCleanUp ();
00019 }
00020 
00021 template<typename real>
00022 void
00023   pcl::BivariatePolynomialT<real>::setDegree (int newDegree)
00024 {
00025   if (newDegree <= 0)
00026   {
00027     degree = -1;
00028     memoryCleanUp();
00029     return;
00030   }
00031   int oldDegree = degree;
00032   degree = newDegree;
00033   if (oldDegree != degree)
00034   {
00035     delete[] parameters;
00036     parameters = new real[getNoOfParameters ()];
00037   }
00038   delete gradient_x; gradient_x = NULL;
00039   delete gradient_y; gradient_y = NULL;
00040 }
00041 
00042 template<typename real>
00043 void
00044   pcl::BivariatePolynomialT<real>::memoryCleanUp ()
00045 {
00046   delete[] parameters; parameters = NULL;
00047   delete gradient_x; gradient_x = NULL;
00048   delete gradient_y; gradient_y = NULL;
00049 }
00050 
00051 template<typename real>
00052 void
00053   pcl::BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other)
00054 {
00055   if (this == &other) return;
00056   if (degree != other.degree) {
00057     memoryCleanUp ();
00058     degree = other.degree;
00059     parameters = new real[getNoOfParameters ()];
00060   }
00061   if (other.gradient_x == NULL) {
00062     delete gradient_x; gradient_x=NULL;
00063     delete gradient_y; gradient_y=NULL;
00064   }
00065   else if (gradient_x==NULL) {
00066     gradient_x = new pcl::BivariatePolynomialT<real> ();
00067     gradient_y = new pcl::BivariatePolynomialT<real> ();
00068   }
00069   real* tmpParameters1 = parameters;
00070   const real* tmpParameters2 = other.parameters;
00071   unsigned int noOfParameters = getNoOfParameters ();
00072   for (unsigned int i=0; i<noOfParameters; i++)
00073     *tmpParameters1++ = *tmpParameters2++;
00074 
00075   if (other.gradient_x != NULL) {
00076     gradient_x->deepCopy (*other.gradient_x);
00077     gradient_y->deepCopy (*other.gradient_y);
00078   }
00079 }
00080 
00081 template<typename real>
00082 void
00083   pcl::BivariatePolynomialT<real>::calculateGradient (bool forceRecalc)
00084 {
00085   if (gradient_x!=NULL && !forceRecalc) {
00086     //cout << "Gradient already exists\n";
00087     return;
00088   }
00089   
00090   if (gradient_x == NULL)
00091     gradient_x = new pcl::BivariatePolynomialT<real> (degree-1);
00092   if (gradient_y == NULL)
00093     gradient_y = new pcl::BivariatePolynomialT<real> (degree-1);
00094   
00095   unsigned int parameterPosDx=0, parameterPosDy=0;
00096   for (int xDegree=degree; xDegree>=0; xDegree--) {
00097     for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) {
00098       if (xDegree > 0) {
00099         gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
00100         parameterPosDx++;
00101       }
00102       if (yDegree > 0) {
00103         gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
00104                                                                         yDegree - 1];
00105         parameterPosDy++;
00106       }
00107     }
00108   }
00109   //cout << "Original polynom: "<<*this<<"\n"<<"d/dx: "<<*gradient_x<<"\n"<<"d/dy: "<<*gradient_y<<"\n";
00110 }
00111 
00112 template<typename real>
00113 real
00114   pcl::BivariatePolynomialT<real>::getValue (real x, real y) const
00115 {
00116   unsigned int parametersSize = getNoOfParameters ();
00117   real* tmpParameter = &parameters[parametersSize-1];
00118   real tmpX=1.0, tmpY, ret=0;
00119   for (int xDegree=0; xDegree<=degree; xDegree++) {
00120     tmpY = 1.0;
00121     for (int yDegree=0; yDegree<=degree-xDegree; yDegree++)
00122     {
00123       ret += (*tmpParameter)*tmpX*tmpY;
00124       tmpY *= y;
00125       tmpParameter--;
00126     }
00127     tmpX *= x;
00128   }
00129   return ret;
00130 }
00131 
00132 template<typename real>
00133 void
00134   pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
00135 {
00136   calculateGradient ();
00137   gradX = gradient_x->getValue (x, y);
00138   gradY = gradient_y->getValue (x, y);
00139   //cout << "Gradient at "<<x<<", "<<y<<" is "<<gradX<<", "<<gradY<<"\n";
00140 }
00141 
00142 //elliptischer Punkt hyperbolischer Punkt parabolischer Punkt
00143 //Bei Funktionen in zwei Ver¨anderlichen kann der Typ anhand der Determinante der Hesse-
00144 //Matrix klassifiziert werden. Ist det(Hf) > 0 (< 0), so handelt es sich um ein Extremum (einen
00145 //Sattelpunkt). F¨ur ein Minimum bzw. ein Maximum ist Spur(Hf) > 0 bzw. < 0 . Verschwindet
00146 //die Determinante und ist die Hesse-Matrix nicht Null, so ist der Punkt parabolisch.
00147 
00148 template<typename real>
00149 void
00150   pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
00151                                                        std::vector<int>& types) const
00152 {
00153   x_values.clear ();
00154   y_values.clear ();
00155   types.clear ();
00156   
00157   if (degree == 2)
00158   {
00159     real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
00160              (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
00161          y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
00162     
00163     if (!pcl_isfinite(x) || !pcl_isfinite(y))
00164       return;
00165     
00166     int type = 2;
00167     real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
00168     //std::cout << "det(H) = "<<det_H<<"\n";
00169     if (det_H > real(0))  // Check Hessian determinant
00170     {
00171       if (parameters[0]+parameters[3] < real(0))  // Check Hessian trace
00172         type = 0;
00173       else
00174         type = 1;
00175     }
00176     x_values.push_back(x);
00177     y_values.push_back(y);
00178     types.push_back(type);
00179     
00180     //real gx, gy;
00181     //((BivariatePolynomialT<real>*)this)->getValueOfGradient (x, y, gx, gy);
00182     //std::cout << "Check: p'("<<x<<", "<<y<<") = ("<<gx<<", "<<gy<<")\n";
00183   }
00184   else
00185   {
00186     std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
00187   }
00188 }
00189 
00190 template<typename real>
00191 std::ostream&
00192   pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
00193 {
00194   real* tmpParameter = p.parameters;
00195   bool first = true;
00196   real currentParameter;
00197   for (int xDegree=p.degree; xDegree>=0; xDegree--) {
00198     for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) {
00199       currentParameter = *tmpParameter;
00200       if (!first) {
00201         os << (currentParameter<0.0?" - ":" + ");
00202         currentParameter = fabs (currentParameter);
00203       }
00204       os << currentParameter;
00205       if (xDegree>0) {
00206         os << "x";
00207         if (xDegree>1)
00208           os<<"^"<<xDegree;
00209       }
00210       if (yDegree>0) {
00211         os << "y";
00212         if (yDegree>1)
00213           os<<"^"<<yDegree;
00214       }
00215       
00216       first = false;
00217       tmpParameter++;
00218     }
00219   }
00220   return os;
00221 }
00222 
00223 template<typename real>
00224 void
00225   pcl::BivariatePolynomialT<real>::writeBinary (std::ostream& os) const
00226 {
00227   os.write ( (char*)&degree, sizeof (int));
00228   unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
00229   os.write ( (char*) (this->parameters), paramCnt * sizeof (real));
00230 }
00231 
00232 template<typename real>
00233 void
00234   pcl::BivariatePolynomialT<real>::writeBinary (const char* filename) const
00235 {
00236   std::ofstream fout (filename);
00237   writeBinary (fout);
00238 }
00239 
00240 template<typename real>
00241 void
00242   pcl::BivariatePolynomialT<real>::readBinary (std::istream& os)
00243 {
00244   memoryCleanUp ();
00245   os.read ( (char*)& (this->degree), sizeof (int));
00246   unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
00247   parameters = new real[paramCnt];
00248   os.read ( (char*)& (*this->parameters), paramCnt * sizeof (real));
00249 }
00250 
00251 template<typename real>
00252 void
00253   pcl::BivariatePolynomialT<real>::readBinary (const char* filename)
00254 {
00255   std::ifstream fin (filename);
00256   readBinary (fin);
00257 }
00258 
00259 
00260 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines