38 #ifndef PCL_POLYNOMIAL_CALCULATIONS_HPP_
39 #define PCL_POLYNOMIAL_CALCULATIONS_HPP_
43 template <
typename real>
50 template <
typename real>
57 template <
typename real>
67 template <
typename real>
75 roots.push_back (0.0);
79 roots.push_back (-b/a);
83 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
84 for (
unsigned int i=0; i<roots.size (); i++)
87 real result = a*x + b;
90 cout <<
"Something went wrong during solving of polynomial "<<a<<
"x + "<<b<<
" = 0\n";
93 cout <<
"Root "<<i<<
" = "<<roots[i]<<
". ("<<a<<
"x^ + "<<b<<
" = "<<result<<
")\n";
100 template <
typename real>
115 roots.push_back (0.0);
117 std::vector<real> tmpRoots;
119 for (
unsigned int i=0; i<tmpRoots.size (); i++)
121 roots.push_back (tmpRoots[i]);
125 real tmp = b*b - 4*a*c;
129 real tmp2 = 1.0/ (2*a);
130 roots.push_back ( (-b + tmp)*tmp2);
131 roots.push_back ( (-b - tmp)*tmp2);
135 roots.push_back (-b/ (2*a));
139 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
140 for (
unsigned int i=0; i<roots.size (); i++)
142 real x=roots[i], x2=x*x;
143 real result = a*x2 + b*x + c;
146 cout <<
"Something went wrong during solving of polynomial "<<a<<
"x^2 + "<<b<<
"x + "<<c<<
" = 0\n";
156 template<
typename real>
171 roots.push_back (0.0);
173 std::vector<real> tmpRoots;
175 for (
unsigned int i=0; i<tmpRoots.size (); i++)
177 roots.push_back (tmpRoots[i]);
185 alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
186 beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
187 alpha2 = alpha*alpha,
188 alpha3 = alpha2*alpha,
192 double resubValue = b/ (3*a);
196 double discriminant = (alpha3/27.0) + 0.25*beta2;
204 roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
205 roots.push_back ( (3.0*beta)/alpha - resubValue);
209 roots.push_back (-resubValue);
212 else if (discriminant > 0)
214 double sqrtDiscriminant = sqrt (discriminant);
215 double d1 = -0.5*beta + sqrtDiscriminant,
216 d2 = -0.5*beta - sqrtDiscriminant;
218 d1 = -pow (-d1, 1.0/3.0);
220 d1 = pow (d1, 1.0/3.0);
223 d2 = -pow (-d2, 1.0/3.0);
225 d2 = pow (d2, 1.0/3.0);
228 roots.push_back (d1 + d2 - resubValue);
232 double tmp1 = sqrt (- (4.0/3.0)*alpha),
233 tmp2 = acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
234 roots.push_back (tmp1*cos (tmp2) - resubValue);
235 roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
236 roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
240 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
241 for (
unsigned int i=0; i<roots.size (); i++)
243 real x=roots[i], x2=x*x, x3=x2*x;
244 real result = a*x3 + b*x2 + c*x + d;
245 if (fabs (result) > 1e-4)
247 cout <<
"Something went wrong:\n";
250 cout <<
"Root "<<i<<
" = "<<roots[i]<<
". ("<<a<<
"x^3 + "<<b<<
"x^2 + "<<c<<
"x + "<<d<<
" = "<<result<<
")\n";
258 template<
typename real>
261 std::vector<real>& roots)
const
274 roots.push_back (0.0);
276 std::vector<real> tmpRoots;
278 for (
unsigned int i=0; i<tmpRoots.size (); i++)
280 roots.push_back (tmpRoots[i]);
284 double root1, root2, root3, root4,
291 alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
292 beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
293 gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
294 alpha2 = alpha*alpha;
297 double resubValue = b/ (4*a);
304 std::vector<real> tmpRoots;
306 for (
unsigned int i=0; i<tmpRoots.size (); i++)
308 double qudraticRoot = tmpRoots[i];
311 roots.push_back (-resubValue);
313 else if (qudraticRoot > 0.0)
315 root1 = sqrt (qudraticRoot);
316 roots.push_back (root1 - resubValue);
317 roots.push_back (-root1 - resubValue);
324 double alpha3 = alpha2*alpha,
326 p = (-alpha2/12.0)-gamma,
327 q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
330 u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
332 u = pow (u, 1.0/3.0);
336 u = -pow (-u, 1.0/3.0);
338 double y = (-5.0/6.0)*alpha - u;
342 double w = alpha + 2.0*y;
358 double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
359 tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
364 root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
365 root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
366 roots.push_back (root1);
367 roots.push_back (root2);
371 root1 = - (b/ (4.0*a)) + 0.5*w;
372 roots.push_back (root1);
378 root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
379 root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
380 roots.push_back (root3);
381 roots.push_back (root4);
385 root3 = - (b/ (4.0*a)) - 0.5*w;
386 roots.push_back (root3);
393 cout << __PRETTY_FUNCTION__ <<
": Found "<<roots.size ()<<
" roots.\n";
394 for (
unsigned int i=0; i<roots.size (); i++)
396 real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
397 real result = a*x4 + b*x3 + c*x2 + d*x + e;
398 if (fabs (result) > 1e-4)
400 cout <<
"Something went wrong:\n";
403 cout <<
"Root "<<i<<
" = "<<roots[i]
404 <<
". ("<<a<<
"x^4 + "<<b<<
"x^3 + "<<c<<
"x^2 + "<<d<<
"x + "<<e<<
" = "<<result<<
")\n";
412 template<
typename real>
415 std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
unsigned int polynomial_degree,
bool& error)
const
424 template<
typename real>
427 std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
unsigned int polynomial_degree,
437 if (parameters_size > samplePoints.size ())
450 Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
452 Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
454 real currentX, currentY, currentZ;
456 real *tmpC =
new real[parameters_size];
457 real* tmpCEndPtr = &tmpC[parameters_size-1];
458 for (
typename std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >::const_iterator it=samplePoints.begin ();
459 it!=samplePoints.end (); ++it)
461 currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
464 real* tmpCPtr = tmpCEndPtr;
466 for (
unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
469 for (
unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
471 * (tmpCPtr--) = tmpX*tmpY;
478 real* APtr = &A(0,0);
481 for (
unsigned int i=0; i<parameters_size; ++i)
483 * (bPtr++) += currentZ * *tmpCPtr1;
486 for (
unsigned int j=0; j<parameters_size; ++j)
488 * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
533 Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
539 parameters = A.inverse () * b;
544 real inversionCheckResult = (A*parameters - b).norm ();
545 if (inversionCheckResult > 1e-5)
551 for (
unsigned int i=0; i<parameters_size; i++)
562 #endif // PCL_POLYNOMIAL_CALCULATIONS_HPP_