polynomial_calculations.hpp
Go to the documentation of this file.
00001 
00002 
00003 template <typename real>
00004 pcl::PolynomialCalculationsT<real>::PolynomialCalculationsT ()
00005 {
00006 }
00007 
00009 
00010 template <typename real>
00011 pcl::PolynomialCalculationsT<real>:: ~PolynomialCalculationsT ()
00012 {
00013 }
00014 
00016 
00017 template <typename real>
00018 inline void 
00019   pcl::PolynomialCalculationsT<real>::Parameters::setZeroValue (real new_zero_value)
00020 {
00021   zero_value = new_zero_value;
00022   sqr_zero_value = zero_value*zero_value;
00023 }
00024 
00026 
00027 template <typename real>
00028 inline void
00029   pcl::PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const
00030 {
00031   //cout << "Trying to solve "<<a<<"x + "<<b<<" = 0\n";
00032 
00033   if (isNearlyZero (b))
00034   {
00035     roots.push_back (0.0);
00036   }
00037   if (!isNearlyZero (a/b))
00038   {
00039     roots.push_back (-b/a);
00040   }
00041   
00042 #if 0
00043   cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
00044   for (unsigned int i=0; i<roots.size (); i++)
00045   {
00046     real x=roots[i];
00047     real result = a*x + b;
00048     if (!isNearlyZero (result))
00049     {
00050       cout << "Something went wrong during solving of polynomial "<<a<<"x + "<<b<<" = 0\n";
00051       //roots.clear ();
00052     }
00053     cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^ + "<<b<<" = "<<result<<")\n";
00054   }
00055 #endif
00056 }
00057 
00059 
00060 template <typename real>
00061 inline void
00062   pcl::PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
00063 {
00064   //cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
00065 
00066   if (isNearlyZero (a))
00067   {
00068     //cout << "Highest order element is 0 => Calling solveLineaqrEquation.\n";
00069     solveLinearEquation (b, c, roots);
00070     return;
00071   }
00072 
00073   if (isNearlyZero (c))
00074   {
00075     roots.push_back (0.0);
00076     //cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n";
00077     std::vector<real> tmpRoots;
00078     solveLinearEquation (a, b, tmpRoots);
00079     for (unsigned int i=0; i<tmpRoots.size (); i++)
00080       if (!isNearlyZero (tmpRoots[i]))
00081         roots.push_back (tmpRoots[i]);
00082     return;
00083   }
00084 
00085   real tmp = b*b - 4*a*c;
00086   if (tmp>0)
00087   {
00088     tmp = sqrt (tmp);
00089     real tmp2 = 1.0/ (2*a);
00090     roots.push_back ( (-b + tmp)*tmp2);
00091     roots.push_back ( (-b - tmp)*tmp2);
00092   }
00093   else if (sqrtIsNearlyZero (tmp))
00094   {
00095     roots.push_back (-b/ (2*a));
00096   }
00097 
00098 #if 0
00099   cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
00100   for (unsigned int i=0; i<roots.size (); i++)
00101   {
00102     real x=roots[i], x2=x*x;
00103     real result = a*x2 + b*x + c;
00104     if (!isNearlyZero (result))
00105     {
00106       cout << "Something went wrong during solving of polynomial "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
00107       //roots.clear ();
00108     }
00109     //cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^2 + "<<b<<"x + "<<c<<" = "<<result<<")\n";
00110   }
00111 #endif
00112 }
00113 
00115 
00116 template<typename real>
00117 inline void
00118   pcl::PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
00119 {
00120   //cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n";
00121 
00122   if (isNearlyZero (a))
00123   {
00124     //cout << "Highest order element is 0 => Calling solveQuadraticEquation.\n";
00125     solveQuadraticEquation (b, c, d, roots);
00126     return;
00127   }
00128 
00129   if (isNearlyZero (d))
00130   {
00131     roots.push_back (0.0);
00132     //cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n";
00133     std::vector<real> tmpRoots;
00134     solveQuadraticEquation (a, b, c, tmpRoots);
00135     for (unsigned int i=0; i<tmpRoots.size (); i++)
00136       if (!isNearlyZero (tmpRoots[i]))
00137         roots.push_back (tmpRoots[i]);
00138     return;
00139   }
00140 
00141   double a2 = a*a,
00142          a3 = a2*a,
00143          b2 = b*b,
00144          b3 = b2*b,
00145          alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
00146          beta  = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
00147          alpha2 = alpha*alpha,
00148          alpha3 = alpha2*alpha,
00149          beta2 = beta*beta;
00150   
00151   // Value for resubstitution:
00152   double resubValue = b/ (3*a);
00153 
00154   //cout << "Trying to solve y^3 + "<<alpha<<"y + "<<beta<<"\n";
00155 
00156   double discriminant = (alpha3/27.0) + 0.25*beta2;
00157 
00158   //cout << "Discriminant is "<<discriminant<<"\n";
00159 
00160   if (isNearlyZero (discriminant))
00161   {
00162     if (!isNearlyZero (alpha) || !isNearlyZero (beta))
00163     {
00164       roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
00165       roots.push_back ( (3.0*beta)/alpha - resubValue);
00166     }
00167     else
00168     {
00169       roots.push_back (-resubValue);
00170     }
00171   }
00172   else if (discriminant > 0)
00173   {
00174     double sqrtDiscriminant = sqrt (discriminant);
00175     double d1 = -0.5*beta + sqrtDiscriminant,
00176            d2 = -0.5*beta - sqrtDiscriminant;
00177     if (d1 < 0)
00178       d1 = -pow (-d1, 1.0/3.0);
00179     else
00180       d1 = pow (d1, 1.0/3.0);
00181 
00182     if (d2 < 0)
00183       d2 = -pow (-d2, 1.0/3.0);
00184     else
00185       d2 = pow (d2, 1.0/3.0);
00186 
00187     //cout << PVAR (d1)<<", "<<PVAR (d2)<<"\n";
00188     roots.push_back (d1 + d2 - resubValue);
00189   }
00190   else
00191   {
00192     double tmp1 = sqrt (- (4.0/3.0)*alpha),
00193            tmp2 = acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
00194     roots.push_back (tmp1*cos (tmp2) - resubValue);
00195     roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
00196     roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
00197   }
00198  
00199 #if 0
00200   cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
00201   for (unsigned int i=0; i<roots.size (); i++)
00202   {
00203     real x=roots[i], x2=x*x, x3=x2*x;
00204     real result = a*x3 + b*x2 + c*x + d;
00205     if (fabs (result) > 1e-4)
00206     {
00207       cout << "Something went wrong:\n";
00208       //roots.clear ();
00209     }
00210     cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = "<<result<<")\n";
00211   }
00212   cout << "\n\n";
00213 #endif
00214 }
00215 
00217 
00218 template<typename real>
00219 inline void
00220   pcl::PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e,
00221                                                             std::vector<real>& roots) const
00222 {
00223   //cout << "Trying to solve "<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = 0\n";
00224 
00225   if (isNearlyZero (a))
00226   {
00227     //cout << "Highest order element is 0 => Calling solveCubicEquation.\n";
00228     solveCubicEquation (b, c, d, e, roots);
00229     return;
00230   } 
00231 
00232   if (isNearlyZero (e))
00233   {
00234     roots.push_back (0.0);
00235     //cout << "Constant element is 0 => Adding root 0 and calling solveCubicEquation.\n";
00236     std::vector<real> tmpRoots;
00237     solveCubicEquation (a, b, c, d, tmpRoots);
00238     for (unsigned int i=0; i<tmpRoots.size (); i++)
00239       if (!isNearlyZero (tmpRoots[i]))
00240         roots.push_back (tmpRoots[i]);
00241     return;
00242   } 
00243 
00244   double root1, root2, root3, root4,
00245          a2 = a*a,
00246          a3 = a2*a,
00247          a4 = a2*a2,
00248          b2 = b*b,
00249          b3 = b2*b,
00250          b4 = b2*b2,
00251          alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
00252          beta  = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
00253          gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
00254          alpha2 = alpha*alpha;
00255   
00256   // Value for resubstitution:
00257   double resubValue = b/ (4*a);
00258 
00259   //cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n";
00260   
00261   if (isNearlyZero (beta))
00262   {  // y^4 + alpha*y^2 + gamma\n";
00263     //cout << "Using beta=0 condition\n";
00264     std::vector<real> tmpRoots;
00265     solveQuadraticEquation (1.0, alpha, gamma, tmpRoots);
00266     for (unsigned int i=0; i<tmpRoots.size (); i++)
00267     {
00268       double qudraticRoot = tmpRoots[i];
00269       if (sqrtIsNearlyZero (qudraticRoot))
00270       {
00271         roots.push_back (-resubValue);
00272       }
00273       else if (qudraticRoot > 0.0)
00274       {
00275         root1 = sqrt (qudraticRoot);
00276         roots.push_back (root1 - resubValue);
00277         roots.push_back (-root1 - resubValue);
00278       }
00279     }
00280   }
00281   else
00282   {
00283     //cout << "beta != 0\n";
00284     double alpha3 = alpha2*alpha,
00285            beta2 = beta*beta,
00286            p = (-alpha2/12.0)-gamma,
00287            q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
00288            q2 = q*q,
00289            p3 = p*p*p,
00290            u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
00291     if (u > 0.0)
00292       u = pow (u, 1.0/3.0);
00293     else if (isNearlyZero (u))
00294       u = 0.0;
00295     else
00296       u = -pow (-u, 1.0/3.0);
00297 
00298     double y = (-5.0/6.0)*alpha - u;
00299     if (!isNearlyZero (u))
00300       y += p/ (3.0*u);
00301 
00302     double w = alpha + 2.0*y;
00303     
00304     if (w > 0)
00305     {
00306       w = sqrt (w);
00307     }
00308     else if (isNearlyZero (w))
00309     {
00310       w = 0;
00311     }
00312     else
00313     {
00314       //cout << "Found no roots\n";
00315       return;
00316     }
00317 
00318     double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
00319            tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
00320     
00321     if (tmp1 > 0)
00322     {
00323       tmp1 = sqrt (tmp1);
00324       root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
00325       root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
00326       roots.push_back (root1);
00327       roots.push_back (root2);
00328     }
00329     else if (isNearlyZero (tmp1))
00330     {
00331       root1 = - (b/ (4.0*a)) + 0.5*w;
00332       roots.push_back (root1);
00333     }
00334 
00335    if (tmp2 > 0)
00336    {
00337       tmp2 = sqrt (tmp2);
00338       root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
00339       root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
00340       roots.push_back (root3);
00341       roots.push_back (root4);
00342     }
00343     else if (isNearlyZero (tmp2))
00344     {
00345       root3 = - (b/ (4.0*a)) - 0.5*w;
00346       roots.push_back (root3);
00347     }
00348    
00349     //cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n";
00350   }
00351   
00352 #if 0
00353   cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
00354   for (unsigned int i=0; i<roots.size (); i++)
00355   {
00356     real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
00357     real result = a*x4 + b*x3 + c*x2 + d*x + e;
00358     if (fabs (result) > 1e-4)
00359     {
00360       cout << "Something went wrong:\n";
00361       //roots.clear ();
00362     }
00363     cout << "Root "<<i<<" = "<<roots[i]
00364          << ". ("<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = "<<result<<")\n";
00365   }
00366   cout << "\n\n";
00367 #endif
00368 }
00369 
00371 
00372 template<typename real>
00373 inline pcl::BivariatePolynomialT<real>
00374   pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
00375       std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree, bool& error) const
00376 {
00377   pcl::BivariatePolynomialT<real> ret;
00378   error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
00379   return ret;
00380 }
00381 
00383 
00384 template<typename real>
00385 inline bool
00386   pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
00387       std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree,
00388       pcl::BivariatePolynomialT<real>& ret) const
00389 {
00390   //MEASURE_FUNCTION_TIME;
00391   unsigned int parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
00392   //cout << PVARN (parameters_size);
00393 
00394   //cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree "
00395   //     << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n";
00396   
00397   if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)?
00398   {
00399     return false;    
00400     // Reduce degree of polynomial
00401     //polynomial_degree = (unsigned int) (0.5f* (sqrtf (8*samplePoints.size ()+1) - 3));
00402     //parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
00403     //cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
00404     //     << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
00405   }
00406   
00407   ret.setDegree (polynomial_degree);
00408   
00409   //double coeffStuffStartTime=-get_time ();
00410   Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
00411   A.setZero();
00412   Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
00413   b.setZero();
00414   real currentX, currentY, currentZ;
00415   real tmpX, tmpY;
00416   real *tmpC = new real[parameters_size];
00417   real* tmpCEndPtr = &tmpC[parameters_size-1];
00418   for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
00419        it!=samplePoints.end (); ++it)
00420   {
00421     currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
00422     //cout << "current point: "<<currentX<<","<<currentY<<" => "<<currentZ<<"\n";
00423     //unsigned int posInC = parameters_size-1;
00424     real* tmpCPtr = tmpCEndPtr;
00425     tmpX = 1.0;
00426     for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
00427     {
00428       tmpY = 1.0;
00429       for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
00430       {
00431         * (tmpCPtr--) = tmpX*tmpY;
00432         //cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC--<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
00433         tmpY *= currentY;
00434       }
00435       tmpX *= currentX;
00436     }
00437     
00438     real* APtr = &A(0,0);
00439     real* bPtr = &b[0];
00440     real* tmpCPtr1=tmpC;
00441     for (unsigned int i=0; i<parameters_size; ++i)
00442     {
00443       * (bPtr++) += currentZ * *tmpCPtr1;
00444       
00445       real* tmpCPtr2=tmpC;
00446       for (unsigned int j=0; j<parameters_size; ++j)
00447       {
00448         * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
00449       }
00450       
00451       ++tmpCPtr1;
00452     }
00453     //A += DMatrix<real>::outProd (tmpC);
00454     //b += currentZ * tmpC;
00455   }
00456   //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
00457        //<< (coeffStuffStartTime+get_time ())*1000<<"ms using constant memory.\n";
00458     //cout << PVARC (A)<<PVARN (b);
00459 
00460 
00461   //double coeffStuffStartTime=-get_time ();
00462   //DMatrix<real> A (parameters_size, parameters_size);
00463   //DVector<real> b (parameters_size);
00464   //real currentX, currentY, currentZ;
00465   //unsigned int posInC;
00466   //real tmpX, tmpY;
00467   //DVector<real> tmpC (parameters_size);
00468   //for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
00469   //     it!=samplePoints.end (); ++it)
00470   //{
00471     //currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
00473     //posInC = parameters_size-1;
00474     //tmpX = 1.0;
00475     //for (unsigned int xDegree=0; xDegree<=polynomial_degree; xDegree++)
00476     //{
00477       //tmpY = 1.0;
00478       //for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; yDegree++)
00479       //{
00480         //tmpC[posInC] = tmpX*tmpY;
00482         //tmpY *= currentY;
00483         //posInC--;
00484       //}
00485       //tmpX *= currentX;
00486     //}
00487     //A += DMatrix<real>::outProd (tmpC);
00488     //b += currentZ * tmpC;
00489   //}
00490   //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
00491        //<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n";
00492   
00493   Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
00494   //double choleskyStartTime=-get_time ();
00495   //parameters = A.choleskySolve (b);
00496   //cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n";
00497 
00498   //double invStartTime=-get_time ();
00499   parameters = A.inverse () * b;
00500   //cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";
00501 
00502   //cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);
00503   
00504   real inversionCheckResult = (A*parameters - b).norm ();
00505   if (inversionCheckResult > 1e-5)
00506   {
00507     //cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
00508     return false;
00509   }
00510   
00511   for (unsigned int i=0; i<parameters_size; i++)
00512     ret.parameters[i] = parameters[i];
00513   
00514   //cout << "Resulting polynomial is "<<ret<<"\n";
00515 
00516   //Test of gradient: ret.calculateGradient ();
00517 
00518   delete [] tmpC;
00519   return true;
00520 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:21