4 # pragma GCC system_header
7 #include <pcl/registration/eigen.h>
11 template<
typename _Scalar >
12 class PolynomialSolver<_Scalar,2> :
public PolynomialSolverBase<_Scalar,2>
15 using PS_Base = PolynomialSolverBase<_Scalar,2>;
16 EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES(
PS_Base )
22 template<
typename OtherPolynomial >
25 compute( poly, hasRealRoot );
29 template<
typename OtherPolynomial >
30 void compute(
const OtherPolynomial& poly,
bool& hasRealRoot)
33 Scalar a2(2 * poly[2]);
34 assert( ZERO != poly[poly.size()-1] );
35 Scalar discriminant ((poly[1] * poly[1]) - (4 * poly[0] * poly[2]));
36 if (ZERO < discriminant)
38 Scalar discriminant_root (std::sqrt (discriminant));
39 m_roots[0] = (-poly[1] - discriminant_root) / (a2) ;
40 m_roots[1] = (-poly[1] + discriminant_root) / (a2) ;
44 if (ZERO == discriminant)
47 m_roots[0] = -poly[1] / a2;
52 Scalar discriminant_root (std::sqrt (-discriminant));
53 m_roots[0] = RootType (-poly[1] / a2, -discriminant_root / a2);
54 m_roots[1] = RootType (-poly[1] / a2, discriminant_root / a2);
60 template<
typename OtherPolynomial >
61 void compute(
const OtherPolynomial& poly)
64 compute(poly, hasRealRoot);
68 using PS_Base::m_roots;
82 template<
typename _Scalar,
int NX=Eigen::Dynamic>
87 using VectorType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
113 template<
typename FunctorType>
117 using Scalar =
typename FunctorType::Scalar;
121 : pnorm(0), g0norm(0), iter(-1), functor(_functor) { }
169 void checkExtremum (
const Eigen::Matrix<Scalar, 4, 1>& coefficients,
Scalar x,
Scalar& xmin,
Scalar& fmin);
170 void moveTo (
Scalar alpha);
176 void changeDirection ();
194 FunctorType &functor;
198 template<
typename FunctorType>
void
201 Scalar y = Eigen::poly_eval(coefficients, x);
202 if(y < fmin) { xmin = x; fmin = y; }
205 template<
typename FunctorType>
void
208 x_alpha = x0 + alpha * p;
215 return (g_alpha.dot (p));
221 if (alpha == f_cache_key)
return f_alpha;
223 f_alpha = functor (x_alpha);
231 if (alpha == df_cache_key)
return df_alpha;
233 if(alpha != g_cache_key)
235 functor.df (x_alpha, g_alpha);
239 df_cache_key = alpha;
243 template<
typename FunctorType>
void
246 if(alpha == f_cache_key && alpha == df_cache_key)
253 if(alpha == f_cache_key || alpha == df_cache_key)
256 df = applyDF (alpha);
261 functor.fdf (x_alpha, f_alpha, g_alpha);
265 df_cache_key = alpha;
270 template<
typename FunctorType>
void
274 Scalar f_alpha, df_alpha;
275 applyFDF (alpha, f_alpha, df_alpha);
283 template<
typename FunctorType>
void
300 status = minimizeOneStep(x);
312 functor.fdf(x, f, gradient);
316 p = gradient * -1/g0norm;
321 x_alpha = x0; x_cache_key = 0;
323 f_alpha = f; f_cache_key = 0;
325 g_alpha = g0; g_cache_key = 0;
327 df_alpha = slope (); df_cache_key = 0;
336 Scalar alpha = 0.0, alpha1;
338 if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0)
346 Scalar del = std::max (-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * std::abs(f0));
347 alpha1 = std::min (1.0, 2.0 * del / (-fp0));
350 alpha1 = std::abs(parameters.step_size);
353 parameters.tau1, parameters.tau2, parameters.tau3,
354 parameters.order, alpha1, alpha);
359 updatePosition(alpha, x, f, gradient);
370 Scalar dxg, dgg, dxdg, dgnorm, A,
B;
378 dxg = dx0.dot (gradient);
379 dgg = dg0.dot (gradient);
380 dxdg = dx0.dot (dg0);
381 dgnorm = dg0.norm ();
386 A = -(1.0 + dgnorm * dgnorm / dxdg) *
B + dgg / dxdg;
404 Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0;
413 template <
typename FunctorType>
417 return functor.checkGradient(gradient);
422 Scalar b, Scalar fb, Scalar fpb,
423 Scalar xmin, Scalar xmax,
427 Scalar y, alpha, ymin, ymax, fmin;
429 ymin = (xmin - a) / (b - a);
430 ymax = (xmax - a) / (b - a);
433 if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; };
435 if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits<Scalar>::infinity ())
440 Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb;
441 Scalar xi = fpa + fpb - 2 * (fb - fa);
442 Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi;
444 Eigen::Matrix<Scalar, 4, 1> coefficients;
445 coefficients << c0, c1, c2, c3;
449 fmin = Eigen::poly_eval (coefficients, ymin);
450 checkExtremum (coefficients, ymax, y, fmin);
453 Eigen::Matrix<Scalar, 3, 1> coefficients2;
454 coefficients2 << c1, 2 * c2, 3 * c3;
456 Eigen::PolynomialSolver<Scalar, 2> solver (coefficients2, real_roots);
459 if ((solver.roots ()).size () == 2)
461 y0 = std::real (solver.roots () [0]);
462 y1 = std::real (solver.roots () [1]);
463 if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; }
464 if (y0 > ymin && y0 < ymax)
465 checkExtremum (coefficients, y0, y, fmin);
466 if (y1 > ymin && y1 < ymax)
467 checkExtremum (coefficients, y1, y, fmin);
469 else if ((solver.roots ()).size () == 1)
471 y0 = std::real (solver.roots () [0]);
472 if (y0 > ymin && y0 < ymax)
473 checkExtremum (coefficients, y0, y, fmin);
481 Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa));
482 Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa));
483 Scalar c = 2 * (fb - fa - fpa);
486 if (fh < fmin) { y = ymax; fmin = fh; }
491 if (z > ymin && z < ymax) {
492 Scalar f = fa + z*(fpa + z*(fb - fa -fpa));
493 if (f < fmin) { y = z; fmin = f; };
498 alpha = a + y * (b - a);
504 Scalar tau1, Scalar tau2, Scalar tau3,
505 int order, Scalar alpha1, Scalar &alpha_new)
507 Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next;
508 Scalar alpha = alpha1, alpha_prev = 0.0;
509 Scalar a, b, fa, fb, fpa, fpb;
512 applyFDF (0.0, f0, fp0);
520 fpa = fp0; fpb = 0.0;
524 while (i++ < parameters.bracket_iters)
526 falpha = applyF (alpha);
528 if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev)
530 a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev;
531 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
535 fpalpha = applyDF (alpha);
538 if (std::abs (fpalpha) <= -sigma * fp0)
546 a = alpha; fa = falpha; fpa = fpalpha;
547 b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev;
551 delta = alpha - alpha_prev;
554 Scalar lower = alpha + delta;
555 Scalar upper = alpha + tau1 * delta;
557 alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev,
558 alpha, falpha, fpalpha, lower, upper, order);
563 falpha_prev = falpha;
564 fpalpha_prev = fpalpha;
568 while (i++ < parameters.section_iters)
573 Scalar lower = a + tau2 * delta;
574 Scalar upper = b - tau3 * delta;
576 alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order);
578 falpha = applyF (alpha);
579 if ((a-alpha)*fpa <= std::numeric_limits<Scalar>::epsilon ()) {
584 if (falpha > f0 + rho * alpha * fp0 || falpha >= fa)
587 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
591 fpalpha = applyDF (alpha);
593 if (std::abs(fpalpha) <= -sigma * fp0)
599 if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0))
601 b = a; fb = fa; fpb = fpa;
602 a = alpha; fa = falpha; fpa = fpalpha;
606 a = alpha; fa = falpha; fpa = fpalpha;