38 #ifndef PCL_POLYNOMIAL_CALCULATIONS_HPP_
39 #define PCL_POLYNOMIAL_CALCULATIONS_HPP_
43 template <
typename real>
53 template <
typename real>
61 roots.push_back (0.0);
65 roots.push_back (-b/a);
69 std::cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
70 for (
unsigned int i=0; i<roots.size (); i++)
73 real result = a*x + b;
76 std::cout <<
"Something went wrong during solving of polynomial "<<a<<
"x + "<<b<<
" = 0\n";
79 std::cout <<
"Root "<<i<<
" = "<<roots[i]<<
". ("<<a<<
"x^ + "<<b<<
" = "<<result<<
")\n";
86 template <
typename real>
101 roots.push_back (0.0);
103 std::vector<real> tmpRoots;
105 for (
const auto& tmpRoot: tmpRoots)
107 roots.push_back (tmpRoot);
111 real tmp = b*b - 4*a*c;
115 real tmp2 = 1.0/ (2*a);
116 roots.push_back ( (-b + tmp)*tmp2);
117 roots.push_back ( (-b - tmp)*tmp2);
121 roots.push_back (-b/ (2*a));
125 std::cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
126 for (
unsigned int i=0; i<roots.size (); i++)
128 real x=roots[i], x2=x*x;
129 real result = a*x2 + b*x + c;
132 std::cout <<
"Something went wrong during solving of polynomial "<<a<<
"x^2 + "<<b<<
"x + "<<c<<
" = 0\n";
142 template<
typename real>
157 roots.push_back (0.0);
159 std::vector<real> tmpRoots;
161 for (
const auto& tmpRoot: tmpRoots)
163 roots.push_back (tmpRoot);
171 alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
172 beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
173 alpha2 = alpha*alpha,
174 alpha3 = alpha2*alpha,
178 double resubValue = b/ (3*a);
182 double discriminant = (alpha3/27.0) + 0.25*beta2;
190 roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
191 roots.push_back ( (3.0*beta)/alpha - resubValue);
195 roots.push_back (-resubValue);
198 else if (discriminant > 0)
200 double sqrtDiscriminant = sqrt (discriminant);
201 double d1 = -0.5*beta + sqrtDiscriminant,
202 d2 = -0.5*beta - sqrtDiscriminant;
204 d1 = -pow (-d1, 1.0/3.0);
206 d1 = pow (d1, 1.0/3.0);
209 d2 = -pow (-d2, 1.0/3.0);
211 d2 = pow (d2, 1.0/3.0);
214 roots.push_back (d1 + d2 - resubValue);
218 double tmp1 = sqrt (- (4.0/3.0)*alpha),
219 tmp2 = std::acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
220 roots.push_back (tmp1*std::cos (tmp2) - resubValue);
221 roots.push_back (-tmp1*std::cos (tmp2 + M_PI/3.0) - resubValue);
222 roots.push_back (-tmp1*std::cos (tmp2 - M_PI/3.0) - resubValue);
226 std::cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
227 for (
unsigned int i=0; i<roots.size (); i++)
229 real x=roots[i], x2=x*x, x3=x2*x;
230 real result = a*x3 + b*x2 + c*x + d;
231 if (std::abs (result) > 1e-4)
233 std::cout <<
"Something went wrong:\n";
236 std::cout <<
"Root "<<i<<
" = "<<roots[i]<<
". ("<<a<<
"x^3 + "<<b<<
"x^2 + "<<c<<
"x + "<<d<<
" = "<<result<<
")\n";
244 template<
typename real>
247 std::vector<real>& roots)
const
260 roots.push_back (0.0);
262 std::vector<real> tmpRoots;
264 for (
const auto& tmpRoot: tmpRoots)
266 roots.push_back (tmpRoot);
270 double root1, root2, root3, root4,
277 alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
278 beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
279 gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
280 alpha2 = alpha*alpha;
283 double resubValue = b/ (4*a);
290 std::vector<real> tmpRoots;
292 for (
const auto& quadraticRoot: tmpRoots)
296 roots.push_back (-resubValue);
298 else if (quadraticRoot > 0.0)
300 root1 = sqrt (quadraticRoot);
301 roots.push_back (root1 - resubValue);
302 roots.push_back (-root1 - resubValue);
309 double alpha3 = alpha2*alpha,
311 p = (-alpha2/12.0)-gamma,
312 q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
315 u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
317 u = pow (u, 1.0/3.0);
321 u = -pow (-u, 1.0/3.0);
323 double y = (-5.0/6.0)*alpha - u;
327 double w = alpha + 2.0*y;
343 double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
344 tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
349 root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
350 root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
351 roots.push_back (root1);
352 roots.push_back (root2);
356 root1 = - (b/ (4.0*a)) + 0.5*w;
357 roots.push_back (root1);
363 root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
364 root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
365 roots.push_back (root3);
366 roots.push_back (root4);
370 root3 = - (b/ (4.0*a)) - 0.5*w;
371 roots.push_back (root3);
378 std::cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
379 for (
unsigned int i=0; i<roots.size (); i++)
381 real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
382 real result = a*x4 + b*x3 + c*x2 + d*x + e;
383 if (std::abs (result) > 1e-4)
385 std::cout <<
"Something went wrong:\n";
388 std::cout <<
"Root "<<i<<
" = "<<roots[i]
389 <<
". ("<<a<<
"x^4 + "<<b<<
"x^3 + "<<c<<
"x^2 + "<<d<<
"x + "<<e<<
" = "<<result<<
")\n";
397 template<
typename real>
400 std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
unsigned int polynomial_degree,
bool& error)
const
409 template<
typename real>
412 std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
unsigned int polynomial_degree,
418 if (parameters_size > samplePoints.size ())
431 Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
433 Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
437 std::vector<real> C (parameters_size);
438 for (
const auto& point: samplePoints)
440 real currentX = point[0], currentY = point[1], currentZ = point[2];
443 auto CRevPtr = C.rbegin ();
445 for (
unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
448 for (
unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree, ++CRevPtr)
450 *CRevPtr = tmpX*tmpY;
457 for (std::size_t i=0; i<parameters_size; ++i)
459 b[i] += currentZ * C[i];
461 for (std::size_t j=i; j<parameters_size; ++j)
463 A (i, j) += C[i] * C[j];
474 for (std::size_t i = 0; i < parameters_size; ++i)
476 for (std::size_t j = 0; j < i; ++j)
482 Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
488 parameters = A.inverse () * b;
493 real inversionCheckResult = (A*parameters - b).norm ();
494 if (inversionCheckResult > 1e-5)
500 std::copy_n(parameters.data(), parameters_size, ret.
parameters);
509 #endif // PCL_POLYNOMIAL_CALCULATIONS_HPP_