5 #pragma GCC system_header
8 #include <unsupported/Eigen/Polynomials>
11 template <
typename _Scalar>
12 class PolynomialSolver<_Scalar, 2> :
public PolynomialSolverBase<_Scalar, 2> {
14 using PS_Base = PolynomialSolverBase<_Scalar, 2>;
15 EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES(
PS_Base)
20 template <
typename OtherPolynomial>
23 compute(poly, hasRealRoot);
27 template <
typename OtherPolynomial>
29 compute(
const OtherPolynomial& poly,
bool& hasRealRoot)
32 Scalar a2(2 * poly[2]);
33 assert(ZERO != poly[poly.size() - 1]);
34 Scalar discriminant((poly[1] * poly[1]) - (4 * poly[0] * poly[2]));
35 if (ZERO < discriminant) {
36 Scalar discriminant_root(std::sqrt(discriminant));
37 m_roots[0] = (-poly[1] - discriminant_root) / (a2);
38 m_roots[1] = (-poly[1] + discriminant_root) / (a2);
42 if (ZERO == discriminant) {
44 m_roots[0] = -poly[1] / a2;
48 Scalar discriminant_root(std::sqrt(-discriminant));
49 m_roots[0] = RootType(-poly[1] / a2, -discriminant_root / a2);
50 m_roots[1] = RootType(-poly[1] / a2, discriminant_root / a2);
56 template <
typename OtherPolynomial>
61 compute(poly, hasRealRoot);
65 using PS_Base::m_roots;
79 template <
typename _Scalar,
int NX = Eigen::Dynamic>
83 using VectorType = Eigen::Matrix<Scalar, InputsAtCompileTime, 1>;
120 template <
typename FunctorType>
123 using Scalar =
typename FunctorType::Scalar;
126 BFGS(FunctorType& _functor) : pnorm(0), g0norm(0), iter(-1), functor(_functor) {}
177 operator=(
const BFGS&);
198 checkExtremum(
const Eigen::Matrix<Scalar, 4, 1>& coefficients,
233 FunctorType& functor;
236 template <
typename FunctorType>
243 Scalar y = Eigen::poly_eval(coefficients, x);
250 template <
typename FunctorType>
254 x_alpha = x0 + alpha * p;
258 template <
typename FunctorType>
262 return (g_alpha.dot(p));
265 template <
typename FunctorType>
269 if (alpha == f_cache_key)
272 f_alpha = functor(x_alpha);
277 template <
typename FunctorType>
281 if (alpha == df_cache_key)
284 if (alpha != g_cache_key) {
285 functor.df(x_alpha, g_alpha);
289 df_cache_key = alpha;
293 template <
typename FunctorType>
297 if (alpha == f_cache_key && alpha == df_cache_key) {
303 if (alpha == f_cache_key || alpha == df_cache_key) {
310 functor.fdf(x_alpha, f_alpha, g_alpha);
314 df_cache_key = alpha;
319 template <
typename FunctorType>
327 Scalar f_alpha, df_alpha;
328 applyFDF(alpha, f_alpha, df_alpha);
336 template <
typename FunctorType>
349 template <
typename FunctorType>
355 status = minimizeOneStep(x);
361 template <
typename FunctorType>
368 functor.fdf(x, f, gradient);
372 p = gradient * -1 / g0norm;
393 template <
typename FunctorType>
397 Scalar alpha = 0.0, alpha1;
399 if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0) {
406 std::max(-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * std::abs(f0));
407 alpha1 = std::min(1.0, 2.0 * del / (-fp0));
410 alpha1 = std::abs(parameters.step_size);
424 updatePosition(alpha, x, f, gradient);
435 Scalar dxg, dgg, dxdg, dgnorm, A,
B;
443 dxg = dx0.dot(gradient);
444 dgg = dg0.dot(gradient);
450 A = -(1.0 + dgnorm * dgnorm / dxdg) *
B + dgg / dxdg;
467 Scalar dir = ((p.dot(gradient)) > 0) ? -1.0 : 1.0;
476 template <
typename FunctorType>
480 return functor.checkGradient(gradient);
483 template <
typename FunctorType>
496 Scalar y, alpha, ymin, ymax, fmin;
498 ymin = (xmin - a) / (b - a);
499 ymax = (xmax - a) / (b - a);
508 if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits<Scalar>::infinity()) {
512 Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb;
513 Scalar xi = fpa + fpb - 2 * (fb - fa);
514 Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi;
516 Eigen::Matrix<Scalar, 4, 1> coefficients;
517 coefficients << c0, c1, c2, c3;
521 fmin = Eigen::poly_eval(coefficients, ymin);
522 checkExtremum(coefficients, ymax, y, fmin);
525 Eigen::Matrix<Scalar, 3, 1> coefficients2;
526 coefficients2 << c1, 2 * c2, 3 * c3;
528 Eigen::PolynomialSolver<Scalar, 2> solver(coefficients2, real_roots);
530 if ((solver.roots()).size() == 2)
532 y0 = std::real(solver.roots()[0]);
533 y1 = std::real(solver.roots()[1]);
539 if (y0 > ymin && y0 < ymax)
540 checkExtremum(coefficients, y0, y, fmin);
541 if (y1 > ymin && y1 < ymax)
542 checkExtremum(coefficients, y1, y, fmin);
544 else if ((solver.roots()).size() == 1)
546 y0 = std::real(solver.roots()[0]);
547 if (y0 > ymin && y0 < ymax)
548 checkExtremum(coefficients, y0, y, fmin);
555 Scalar fl = fa + ymin * (fpa + ymin * (fb - fa - fpa));
556 Scalar fh = fa + ymax * (fpa + ymax * (fb - fa - fpa));
557 Scalar c = 2 * (fb - fa - fpa);
569 if (z > ymin && z < ymax) {
570 Scalar f = fa + z * (fpa + z * (fb - fa - fpa));
579 alpha = a + y * (b - a);
583 template <
typename FunctorType>
594 Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next;
595 Scalar alpha = alpha1, alpha_prev = 0.0;
596 Scalar a, b, fa, fb, fpa, fpb;
599 applyFDF(0.0, f0, fp0);
614 while (i++ < parameters.bracket_iters) {
615 falpha = applyF(alpha);
617 if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev) {
623 fpb = std::numeric_limits<Scalar>::quiet_NaN();
627 fpalpha = applyDF(alpha);
630 if (std::abs(fpalpha) <= -sigma * fp0) {
645 delta = alpha - alpha_prev;
648 Scalar lower = alpha + delta;
649 Scalar upper = alpha + tau1 * delta;
651 alpha_next = interpolate(alpha_prev,
663 falpha_prev = falpha;
664 fpalpha_prev = fpalpha;
668 while (i++ < parameters.section_iters) {
672 Scalar lower = a + tau2 * delta;
673 Scalar upper = b - tau3 * delta;
675 alpha = interpolate(a, fa, fpa, b, fb, fpb, lower, upper, order);
677 falpha = applyF(alpha);
678 if ((a - alpha) * fpa <= std::numeric_limits<Scalar>::epsilon()) {
683 if (falpha > f0 + rho * alpha * fp0 || falpha >= fa) {
687 fpb = std::numeric_limits<Scalar>::quiet_NaN();
690 fpalpha = applyDF(alpha);
692 if (std::abs(fpalpha) <= -sigma * fp0) {
697 if (((b - a) >= 0 && fpalpha >= 0) || ((b - a) <= 0 && fpalpha <= 0)) {
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
BFGSSpace::Status minimize(FVectorType &x)
BFGSSpace::Status testGradient()
typename FunctorType::Scalar Scalar
BFGSSpace::Status minimizeInit(FVectorType &x)
BFGSSpace::Status minimizeOneStep(FVectorType &x)
BFGSSpace::Status testGradient(Scalar)
typename FunctorType::VectorType FVectorType
void resetParameters(void)
BFGS(FunctorType &_functor)
void compute(const OtherPolynomial &poly)
void compute(const OtherPolynomial &poly, bool &hasRealRoot)
Computes the complex roots of a new polynomial.
PolynomialSolverBase< _Scalar, 2 > PS_Base
PolynomialSolver(const OtherPolynomial &poly, bool &hasRealRoot)
virtual ~PolynomialSolver()
@ NegativeGradientEpsilon
Defines all the PCL and non-PCL macros used.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
virtual void fdf(const VectorType &x, Scalar &f, VectorType &df)=0
virtual ~BFGSDummyFunctor()
BFGSDummyFunctor(int inputs)
virtual BFGSSpace::Status checkGradient(const VectorType &g)
virtual void df(const VectorType &x, VectorType &df)=0
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > VectorType
virtual double operator()(const VectorType &x)=0