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;
72 template<
typename _Scalar,
int NX=Eigen::Dynamic>
77 using VectorType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
112 template<
typename FunctorType>
116 using Scalar =
typename FunctorType::Scalar;
120 : pnorm(0), g0norm(0), iter(-1), functor(_functor) { }
166 void checkExtremum (
const Eigen::Matrix<Scalar, 4, 1>& coefficients,
Scalar x,
Scalar& xmin,
Scalar& fmin);
167 void moveTo (
Scalar alpha);
173 void changeDirection ();
191 FunctorType &functor;
195 template<
typename FunctorType>
void
198 Scalar y = Eigen::poly_eval(coefficients, x);
199 if(y < fmin) { xmin = x; fmin = y; }
202 template<
typename FunctorType>
void
205 x_alpha = x0 + alpha * p;
212 return (g_alpha.dot (p));
218 if (alpha == f_cache_key)
return f_alpha;
220 f_alpha = functor (x_alpha);
228 if (alpha == df_cache_key)
return df_alpha;
230 if(alpha != g_cache_key)
232 functor.df (x_alpha, g_alpha);
236 df_cache_key = alpha;
240 template<
typename FunctorType>
void
243 if(alpha == f_cache_key && alpha == df_cache_key)
250 if(alpha == f_cache_key || alpha == df_cache_key)
253 df = applyDF (alpha);
258 functor.fdf (x_alpha, f_alpha, g_alpha);
262 df_cache_key = alpha;
267 template<
typename FunctorType>
void
271 Scalar f_alpha, df_alpha;
272 applyFDF (alpha, f_alpha, df_alpha);
280 template<
typename FunctorType>
void
297 status = minimizeOneStep(x);
309 functor.fdf(x, f, gradient);
313 p = gradient * -1/g0norm;
318 x_alpha = x0; x_cache_key = 0;
320 f_alpha = f; f_cache_key = 0;
322 g_alpha = g0; g_cache_key = 0;
324 df_alpha = slope (); df_cache_key = 0;
333 Scalar alpha = 0.0, alpha1;
335 if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0)
343 Scalar del = std::max (-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * std::abs(f0));
344 alpha1 = std::min (1.0, 2.0 * del / (-fp0));
347 alpha1 = std::abs(parameters.step_size);
350 parameters.tau1, parameters.tau2, parameters.tau3,
351 parameters.order, alpha1, alpha);
356 updatePosition(alpha, x, f, gradient);
367 Scalar dxg, dgg, dxdg, dgnorm, A,
B;
375 dxg = dx0.dot (gradient);
376 dgg = dg0.dot (gradient);
377 dxdg = dx0.dot (dg0);
378 dgnorm = dg0.norm ();
383 A = -(1.0 + dgnorm * dgnorm / dxdg) *
B + dgg / dxdg;
401 Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0;
417 if(gradient.norm () < epsilon)
426 Scalar b, Scalar fb, Scalar fpb,
427 Scalar xmin, Scalar xmax,
431 Scalar y, alpha, ymin, ymax, fmin;
433 ymin = (xmin - a) / (b - a);
434 ymax = (xmax - a) / (b - a);
437 if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; };
439 if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits<Scalar>::infinity ())
444 Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb;
445 Scalar xi = fpa + fpb - 2 * (fb - fa);
446 Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi;
448 Eigen::Matrix<Scalar, 4, 1> coefficients;
449 coefficients << c0, c1, c2, c3;
453 fmin = Eigen::poly_eval (coefficients, ymin);
454 checkExtremum (coefficients, ymax, y, fmin);
457 Eigen::Matrix<Scalar, 3, 1> coefficients2;
458 coefficients2 << c1, 2 * c2, 3 * c3;
460 Eigen::PolynomialSolver<Scalar, 2> solver (coefficients2, real_roots);
463 if ((solver.roots ()).size () == 2)
465 y0 = std::real (solver.roots () [0]);
466 y1 = std::real (solver.roots () [1]);
467 if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; }
468 if (y0 > ymin && y0 < ymax)
469 checkExtremum (coefficients, y0, y, fmin);
470 if (y1 > ymin && y1 < ymax)
471 checkExtremum (coefficients, y1, y, fmin);
473 else if ((solver.roots ()).size () == 1)
475 y0 = std::real (solver.roots () [0]);
476 if (y0 > ymin && y0 < ymax)
477 checkExtremum (coefficients, y0, y, fmin);
485 Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa));
486 Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa));
487 Scalar c = 2 * (fb - fa - fpa);
490 if (fh < fmin) { y = ymax; fmin = fh; }
495 if (z > ymin && z < ymax) {
496 Scalar f = fa + z*(fpa + z*(fb - fa -fpa));
497 if (f < fmin) { y = z; fmin = f; };
502 alpha = a + y * (b - a);
508 Scalar tau1, Scalar tau2, Scalar tau3,
509 int order, Scalar alpha1, Scalar &alpha_new)
511 Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next;
512 Scalar alpha = alpha1, alpha_prev = 0.0;
513 Scalar a, b, fa, fb, fpa, fpb;
516 applyFDF (0.0, f0, fp0);
524 fpa = fp0; fpb = 0.0;
528 while (i++ < parameters.bracket_iters)
530 falpha = applyF (alpha);
532 if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev)
534 a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev;
535 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
539 fpalpha = applyDF (alpha);
542 if (std::abs (fpalpha) <= -sigma * fp0)
550 a = alpha; fa = falpha; fpa = fpalpha;
551 b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev;
555 delta = alpha - alpha_prev;
558 Scalar lower = alpha + delta;
559 Scalar upper = alpha + tau1 * delta;
561 alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev,
562 alpha, falpha, fpalpha, lower, upper, order);
567 falpha_prev = falpha;
568 fpalpha_prev = fpalpha;
572 while (i++ < parameters.section_iters)
577 Scalar lower = a + tau2 * delta;
578 Scalar upper = b - tau3 * delta;
580 alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order);
582 falpha = applyF (alpha);
583 if ((a-alpha)*fpa <= std::numeric_limits<Scalar>::epsilon ()) {
588 if (falpha > f0 + rho * alpha * fp0 || falpha >= fa)
591 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
595 fpalpha = applyDF (alpha);
597 if (std::abs(fpalpha) <= -sigma * fp0)
603 if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0))
605 b = a; fb = fa; fpb = fpa;
606 a = alpha; fa = falpha; fpa = fpalpha;
610 a = alpha; fa = falpha; fpa = fpalpha;