41#include <pcl/common/polynomial_calculations.h>
47template <
typename real>
56template <
typename real>
64 roots.push_back (0.0);
68 roots.push_back (-b/a);
73 for (
unsigned int i=0; i<
roots.
size (); i++)
79 std::cout <<
"Something went wrong during solving of polynomial "<<a<<
"x + "<<b<<
" = 0\n";
82 std::cout <<
"Root "<<i<<
" = "<<
roots[i]<<
". ("<<a<<
"x^ + "<<b<<
" = "<<
result<<
")\n";
88template <
typename real>
103 roots.push_back (0.0);
123 roots.push_back (-b/ (2*a));
128 for (
unsigned int i=0; i<
roots.
size (); i++)
134 std::cout <<
"Something went wrong during solving of polynomial "<<a<<
"x^2 + "<<b<<
"x + "<<
c<<
" = 0\n";
143template<
typename real>
158 roots.push_back (0.0);
173 beta = (2*
b3/ (27.0*
a3)) - ( (b*
c)/ (3.0*
a2)) + (d/a),
228 for (
unsigned int i=0; i<
roots.
size (); i++)
234 std::cout <<
"Something went wrong:\n";
237 std::cout <<
"Root "<<i<<
" = "<<
roots[i]<<
". ("<<a<<
"x^3 + "<<b<<
"x^2 + "<<
c<<
"x + "<<d<<
" = "<<
result<<
")\n";
244template<
typename real>
247 std::vector<real>&
roots)
const
260 roots.push_back (0.0);
277 beta = (
b3/ (8.0*
a3)) - ( (b*
c)/ (2.0*
a2)) + (d/a),
278 gamma = ( (-3.0*
b4)/ (256.0*
a4)) + ( (
c*
b2)/ (16.0*
a3)) - ( (b*d)/ (4.0*
a2)) + (
e/a),
314 u = (0.5*
q) +
sqrt ( (0.25*
q2)+ (p3/27.0));
316 u =
pow (u, 1.0/3.0);
320 u = -
pow (-u, 1.0/3.0);
322 double y = (-5.0/6.0)*
alpha - u;
326 double w =
alpha + 2.0*y;
348 double root1 = - (b/ (4.0*a)) + 0.5* (w+
tmp1);
349 double root2 = - (b/ (4.0*a)) + 0.5* (w-
tmp1);
355 double root1 = - (b/ (4.0*a)) + 0.5*w;
362 double root3 = - (b/ (4.0*a)) + 0.5* (-w+
tmp2);
363 double root4 = - (b/ (4.0*a)) + 0.5* (-w-
tmp2);
369 double root3 = - (b/ (4.0*a)) - 0.5*w;
378 for (
unsigned int i=0; i<
roots.
size (); i++)
380 real x=
roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
384 std::cout <<
"Something went wrong:\n";
387 std::cout <<
"Root "<<i<<
" = "<<
roots[i]
388 <<
". ("<<a<<
"x^4 + "<<b<<
"x^3 + "<<
c<<
"x^2 + "<<d<<
"x + "<<
e<<
" = "<<
result<<
")\n";
395template<
typename real>
398 std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >&
samplePoints,
unsigned int polynomial_degree,
bool& error)
const
406template<
typename real>
460 A (i, j) += C[i] * C[j];
473 for (std::size_t j = 0; j < i; ++j)
479 Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
485 parameters =
A.inverse () * b;
static unsigned int getNoOfParametersFromDegree(int n)
How many parameters has a bivariate polynomial of the given degree.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
void solveCubicEquation(real a, real b, real c, real d, std::vector< real > &roots) const
Solves an equation of the form ax^3 + bx^2 + cx + d = 0 See http://en.wikipedia.org/wiki/Cubic_equati...
bool isNearlyZero(real d) const
check if std::abs(d)<zeroValue
BivariatePolynomialT< real > bivariatePolynomialApproximation(std::vector< Eigen::Matrix< real, 3, 1 >, Eigen::aligned_allocator< Eigen::Matrix< real, 3, 1 > > > &samplePoints, unsigned int polynomial_degree, bool &error) const
Get the bivariate polynomial approximation for Z(X,Y) from the given sample points.
bool sqrtIsNearlyZero(real d) const
check if sqrt(std::abs(d))<zeroValue
void solveQuarticEquation(real a, real b, real c, real d, real e, std::vector< real > &roots) const
Solves an equation of the form ax^4 + bx^3 + cx^2 +dx + e = 0 See http://en.wikipedia....
void solveQuadraticEquation(real a, real b, real c, std::vector< real > &roots) const
Solves an equation of the form ax^2 + bx + c = 0 See http://en.wikipedia.org/wiki/Quadratic_equation.
void solveLinearEquation(real a, real b, std::vector< real > &roots) const
Solves an equation of the form ax + b = 0.
void setZeroValue(real new_zero_value)
Set zero_value.
real sqr_zero_value
sqr of the above
real zero_value
Every value below this is considered to be zero.