1 #ifndef PCL_FOR_EIGEN_BFGS_H
2 #define PCL_FOR_EIGEN_BFGS_H
5 # pragma GCC system_header
8 #include <pcl/registration/eigen.h>
12 template<
typename _Scalar >
13 class PolynomialSolver<_Scalar,2> :
public PolynomialSolverBase<_Scalar,2>
16 typedef PolynomialSolverBase<_Scalar,2>
PS_Base;
17 EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES(
PS_Base )
21 virtual ~PolynomialSolver () {}
23 template<
typename OtherPolynomial >
26 compute( poly, hasRealRoot );
30 template<
typename OtherPolynomial >
31 void compute(
const OtherPolynomial& poly,
bool& hasRealRoot)
34 Scalar a2(2 * poly[2]);
35 assert( ZERO != poly[poly.size()-1] );
36 Scalar discriminant ((poly[1] * poly[1]) - (4 * poly[0] * poly[2]));
37 if (ZERO < discriminant)
39 Scalar discriminant_root (std::sqrt (discriminant));
40 m_roots[0] = (-poly[1] - discriminant_root) / (a2) ;
41 m_roots[1] = (-poly[1] + discriminant_root) / (a2) ;
45 if (ZERO == discriminant)
48 m_roots[0] = -poly[1] / a2;
53 Scalar discriminant_root (std::sqrt (-discriminant));
54 m_roots[0] = RootType (-poly[1] / a2, -discriminant_root / a2);
55 m_roots[1] = RootType (-poly[1] / a2, discriminant_root / a2);
61 template<
typename OtherPolynomial >
62 void compute(
const OtherPolynomial& poly)
65 compute(poly, hasRealRoot);
69 using PS_Base::m_roots;
73 template<
typename _Scalar,
int NX=Eigen::Dynamic>
78 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1>
VectorType;
113 template<
typename FunctorType>
117 typedef typename FunctorType::Scalar
Scalar;
121 : pnorm(0), g0norm(0), iter(-1), functor(_functor) { }
167 void checkExtremum (
const Eigen::Matrix<Scalar, 4, 1>& coefficients,
Scalar x,
Scalar& xmin,
Scalar& fmin);
168 void moveTo (
Scalar alpha);
174 void changeDirection ();
192 FunctorType &functor;
196 template<
typename FunctorType>
void
199 Scalar y = Eigen::poly_eval(coefficients, x);
200 if(y < fmin) { xmin = x; fmin = y; }
203 template<
typename FunctorType>
void
206 x_alpha = x0 + alpha * p;
213 return (g_alpha.dot (p));
219 if (alpha == f_cache_key)
return f_alpha;
221 f_alpha = functor (x_alpha);
229 if (alpha == df_cache_key)
return df_alpha;
231 if(alpha != g_cache_key)
233 functor.df (x_alpha, g_alpha);
237 df_cache_key = alpha;
241 template<
typename FunctorType>
void
244 if(alpha == f_cache_key && alpha == df_cache_key)
251 if(alpha == f_cache_key || alpha == df_cache_key)
254 df = applyDF (alpha);
259 functor.fdf (x_alpha, f_alpha, g_alpha);
263 df_cache_key = alpha;
268 template<
typename FunctorType>
void
272 Scalar f_alpha, df_alpha;
273 applyFDF (alpha, f_alpha, df_alpha);
281 template<
typename FunctorType>
void
298 status = minimizeOneStep(x);
310 functor.fdf(x, f, gradient);
314 p = gradient * -1/g0norm;
319 x_alpha = x0; x_cache_key = 0;
321 f_alpha = f; f_cache_key = 0;
323 g_alpha = g0; g_cache_key = 0;
325 df_alpha = slope (); df_cache_key = 0;
334 Scalar alpha = 0.0, alpha1;
336 if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0)
344 Scalar del = std::max (-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * fabs(f0));
345 alpha1 = std::min (1.0, 2.0 * del / (-fp0));
348 alpha1 = fabs(parameters.step_size);
351 parameters.tau1, parameters.tau2, parameters.tau3,
352 parameters.order, alpha1, alpha);
357 updatePosition(alpha, x, f, gradient);
368 Scalar dxg, dgg, dxdg, dgnorm, A,
B;
376 dxg = dx0.dot (gradient);
377 dgg = dg0.dot (gradient);
378 dxdg = dx0.dot (dg0);
379 dgnorm = dg0.norm ();
384 A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
402 Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0;
418 if(gradient.norm () < epsilon)
427 Scalar b, Scalar fb, Scalar fpb,
428 Scalar xmin, Scalar xmax,
432 Scalar y, alpha, ymin, ymax, fmin;
434 ymin = (xmin - a) / (b - a);
435 ymax = (xmax - a) / (b - a);
438 if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; };
440 if (order > 2 && !(fpb != fpb) && fpb != std::numeric_limits<Scalar>::infinity ())
445 Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb;
446 Scalar xi = fpa + fpb - 2 * (fb - fa);
447 Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi;
449 Eigen::Matrix<Scalar, 4, 1> coefficients;
450 coefficients << c0, c1, c2, c3;
454 fmin = Eigen::poly_eval (coefficients, ymin);
455 checkExtremum (coefficients, ymax, y, fmin);
458 Eigen::Matrix<Scalar, 3, 1> coefficients2;
459 coefficients2 << c1, 2 * c2, 3 * c3;
461 Eigen::PolynomialSolver<Scalar, 2> solver (coefficients2, real_roots);
464 if ((solver.roots ()).size () == 2)
466 y0 = std::real (solver.roots () [0]);
467 y1 = std::real (solver.roots () [1]);
468 if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; }
469 if (y0 > ymin && y0 < ymax)
470 checkExtremum (coefficients, y0, y, fmin);
471 if (y1 > ymin && y1 < ymax)
472 checkExtremum (coefficients, y1, y, fmin);
474 else if ((solver.roots ()).size () == 1)
476 y0 = std::real (solver.roots () [0]);
477 if (y0 > ymin && y0 < ymax)
478 checkExtremum (coefficients, y0, y, fmin);
486 Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa));
487 Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa));
488 Scalar c = 2 * (fb - fa - fpa);
491 if (fh < fmin) { y = ymax; fmin = fh; }
496 if (z > ymin && z < ymax) {
497 Scalar f = fa + z*(fpa + z*(fb - fa -fpa));
498 if (f < fmin) { y = z; fmin = f; };
503 alpha = a + y * (b - a);
509 Scalar tau1, Scalar tau2, Scalar tau3,
510 int order, Scalar alpha1, Scalar &alpha_new)
512 Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next;
513 Scalar alpha = alpha1, alpha_prev = 0.0;
514 Scalar a, b, fa, fb, fpa, fpb;
517 applyFDF (0.0, f0, fp0);
525 fpa = fp0; fpb = 0.0;
529 while (i++ < parameters.bracket_iters)
531 falpha = applyF (alpha);
533 if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev)
535 a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev;
536 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
540 fpalpha = applyDF (alpha);
543 if (fabs (fpalpha) <= -sigma * fp0)
551 a = alpha; fa = falpha; fpa = fpalpha;
552 b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev;
556 delta = alpha - alpha_prev;
559 Scalar lower = alpha + delta;
560 Scalar upper = alpha + tau1 * delta;
562 alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev,
563 alpha, falpha, fpalpha, lower, upper, order);
568 falpha_prev = falpha;
569 fpalpha_prev = fpalpha;
573 while (i++ < parameters.section_iters)
578 Scalar lower = a + tau2 * delta;
579 Scalar upper = b - tau3 * delta;
581 alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order);
583 falpha = applyF (alpha);
584 if ((a-alpha)*fpa <= std::numeric_limits<Scalar>::epsilon ()) {
589 if (falpha > f0 + rho * alpha * fp0 || falpha >= fa)
592 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
596 fpalpha = applyDF (alpha);
598 if (fabs(fpalpha) <= -sigma * fp0)
604 if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0))
606 b = a; fb = fa; fpb = fpa;
607 a = alpha; fa = falpha; fpa = fpalpha;
611 a = alpha; fa = falpha; fpa = fpalpha;
617 #endif // PCL_FOR_EIGEN_BFGS_H
void compute(const OtherPolynomial &poly)
BFGS(FunctorType &_functor)
FunctorType::Scalar Scalar
PolynomialSolverBase< _Scalar, 2 > PS_Base
FunctorType::VectorType FVectorType
void compute(const OtherPolynomial &poly, bool &hasRealRoot)
Computes the complex roots of a new polynomial.
virtual ~BFGSDummyFunctor()
BFGSSpace::Status minimize(FVectorType &x)
PolynomialSolver(const OtherPolynomial &poly, bool &hasRealRoot)
virtual double operator()(const VectorType &x)=0
BFGSSpace::Status testGradient(Scalar epsilon)
BFGSDummyFunctor(int inputs)
virtual void fdf(const VectorType &x, Scalar &f, VectorType &df)=0
BFGSSpace::Status minimizeOneStep(FVectorType &x)
void resetParameters(void)
BFGSSpace::Status minimizeInit(FVectorType &x)
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > VectorType
virtual void df(const VectorType &x, VectorType &df)=0
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlin...