1 #pragma once
2 #include <pcl/pcl_macros.h>
3 
4 #if defined __GNUC__
5 #pragma GCC system_header
6 #endif
7 
8 #include <unsupported/Eigen/Polynomials> // for PolynomialSolver, PolynomialSolverBase
9 
10 namespace Eigen {
11 template <typename _Scalar>
12 class PolynomialSolver<_Scalar, 2> : public PolynomialSolverBase<_Scalar, 2> {
13 public:
14   using PS_Base = PolynomialSolverBase<_Scalar, 2>;
EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES(PS_Base)15   EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES(PS_Base)
16 
17 public:
18   virtual ~PolynomialSolver() {}
19 
20   template <typename OtherPolynomial>
PolynomialSolver(const OtherPolynomial & poly,bool & hasRealRoot)21   inline PolynomialSolver(const OtherPolynomial& poly, bool& hasRealRoot)
22   {
23     compute(poly, hasRealRoot);
24   }
25 
26   /** Computes the complex roots of a new polynomial. */
27   template <typename OtherPolynomial>
28   void
compute(const OtherPolynomial & poly,bool & hasRealRoot)29   compute(const OtherPolynomial& poly, bool& hasRealRoot)
30   {
31     const Scalar ZERO(0);
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(::sqrt(discriminant));
37       m_roots[0] = (-poly[1] - discriminant_root) / (a2);
38       m_roots[1] = (-poly[1] + discriminant_root) / (a2);
39       hasRealRoot = true;
40     }
41     else {
42       if (ZERO == discriminant) {
43         m_roots.resize(1);
44         m_roots[0] = -poly[1] / a2;
45         hasRealRoot = true;
46       }
47       else {
48         Scalar discriminant_root(::sqrt(-discriminant));
49         m_roots[0] = RootType(-poly[1] / a2, -discriminant_root / a2);
50         m_roots[1] = RootType(-poly[1] / a2, discriminant_root / a2);
51         hasRealRoot = false;
52       }
53     }
54   }
55 
56   template <typename OtherPolynomial>
57   void
compute(const OtherPolynomial & poly)58   compute(const OtherPolynomial& poly)
59   {
60     bool hasRealRoot;
61     compute(poly, hasRealRoot);
62   }
63 
64 protected:
65   using PS_Base::m_roots;
66 };
67 } // namespace Eigen
68 
69 namespace BFGSSpace {
70 enum Status {
71   NegativeGradientEpsilon = -3,
72   NotStarted = -2,
73   Running = -1,
74   Success = 0,
75   NoProgress = 1
76 };
77 }
78 
79 template <typename _Scalar, int NX = Eigen::Dynamic>
80 struct BFGSDummyFunctor {
81   using Scalar = _Scalar;
82   enum { InputsAtCompileTime = NX };
83   using VectorType = Eigen::Matrix<Scalar, InputsAtCompileTime, 1>;
84 
85   const int m_inputs;
86 
BFGSDummyFunctorBFGSDummyFunctor87   BFGSDummyFunctor() : m_inputs(InputsAtCompileTime) {}
BFGSDummyFunctorBFGSDummyFunctor88   BFGSDummyFunctor(int inputs) : m_inputs(inputs) {}
89 
~BFGSDummyFunctorBFGSDummyFunctor90   virtual ~BFGSDummyFunctor() {}
91   int
inputsBFGSDummyFunctor92   inputs() const
93   {
94     return m_inputs;
95   }
96 
97   virtual double
98   operator()(const VectorType& x) = 0;
99   virtual void
100   df(const VectorType& x, VectorType& df) = 0;
101   virtual void
102   fdf(const VectorType& x, Scalar& f, VectorType& df) = 0;
103   virtual BFGSSpace::Status
checkGradientBFGSDummyFunctor104   checkGradient(const VectorType& g)
105   {
106     return BFGSSpace::NotStarted;
107   };
108 };
109 
110 /**
111  * BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving
112  * unconstrained nonlinear optimization problems.
113  * For further details please visit: http://en.wikipedia.org/wiki/BFGS_method
114  * The method provided here is almost similar to the one provided by GSL.
115  * It reproduces Fletcher's original algorithm in Practical Methods of Optimization
116  * algorithms : 2.6.2 and 2.6.4 and uses the same politics in GSL with cubic
117  * interpolation whenever it is possible else falls to quadratic interpolation for
118  * alpha parameter.
119  */
120 template <typename FunctorType>
121 class BFGS {
122 public:
123   using Scalar = typename FunctorType::Scalar;
124   using FVectorType = typename FunctorType::VectorType;
125 
BFGS(FunctorType & _functor)126   BFGS(FunctorType& _functor) : pnorm(0), g0norm(0), iter(-1), functor(_functor) {}
127 
128   using Index = Eigen::DenseIndex;
129 
130   struct Parameters {
ParametersParameters131     Parameters()
132     : max_iters(400)
133     , bracket_iters(100)
134     , section_iters(100)
135     , rho(0.01)
136     , sigma(0.01)
137     , tau1(9)
138     , tau2(0.05)
139     , tau3(0.5)
140     , step_size(1)
141     , order(3)
142     {}
143     Index max_iters; // maximum number of function evaluation
144     Index bracket_iters;
145     Index section_iters;
146     Scalar rho;
147     Scalar sigma;
148     Scalar tau1;
149     Scalar tau2;
150     Scalar tau3;
151     Scalar step_size;
152     Index order;
153   };
154 
155   BFGSSpace::Status
156   minimize(FVectorType& x);
157   BFGSSpace::Status
158   minimizeInit(FVectorType& x);
159   BFGSSpace::Status
160   minimizeOneStep(FVectorType& x);
161   BFGSSpace::Status
162   testGradient();
163   PCL_DEPRECATED(1, 13, "Use `testGradient()` instead")
testGradient(Scalar)164   BFGSSpace::Status testGradient(Scalar) { return testGradient(); }
165   void
resetParameters(void)166   resetParameters(void)
167   {
168     parameters = Parameters();
169   }
170 
171   Parameters parameters;
172   Scalar f;
173   FVectorType gradient;
174 
175 private:
176   BFGS&
177   operator=(const BFGS&);
178   BFGSSpace::Status
179   lineSearch(Scalar rho,
180              Scalar sigma,
181              Scalar tau1,
182              Scalar tau2,
183              Scalar tau3,
184              int order,
185              Scalar alpha1,
186              Scalar& alpha_new);
187   Scalar
188   interpolate(Scalar a,
189               Scalar fa,
190               Scalar fpa,
191               Scalar b,
192               Scalar fb,
193               Scalar fpb,
194               Scalar xmin,
195               Scalar xmax,
196               int order);
197   void
198   checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients,
199                 Scalar x,
200                 Scalar& xmin,
201                 Scalar& fmin);
202   void
203   moveTo(Scalar alpha);
204   Scalar
205   slope();
206   Scalar
207   applyF(Scalar alpha);
208   Scalar
209   applyDF(Scalar alpha);
210   void
211   applyFDF(Scalar alpha, Scalar& f, Scalar& df);
212   void
213   updatePosition(Scalar alpha, FVectorType& x, Scalar& f, FVectorType& g);
214   void
215   changeDirection();
216 
217   Scalar delta_f, fp0;
218   FVectorType x0, dx0, dg0, g0, dx, p;
219   Scalar pnorm, g0norm;
220 
221   Scalar f_alpha;
222   Scalar df_alpha;
223   FVectorType x_alpha;
224   FVectorType g_alpha;
225 
226   // cache "keys"
227   Scalar f_cache_key;
228   Scalar df_cache_key;
229   Scalar x_cache_key;
230   Scalar g_cache_key;
231 
232   Index iter;
233   FunctorType& functor;
234 };
235 
236 template <typename FunctorType>
237 void
checkExtremum(const Eigen::Matrix<Scalar,4,1> & coefficients,Scalar x,Scalar & xmin,Scalar & fmin)238 BFGS<FunctorType>::checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients,
239                                  Scalar x,
240                                  Scalar& xmin,
241                                  Scalar& fmin)
242 {
243   Scalar y = Eigen::poly_eval(coefficients, x);
244   if (y < fmin) {
245     xmin = x;
246     fmin = y;
247   }
248 }
249 
250 template <typename FunctorType>
251 void
moveTo(Scalar alpha)252 BFGS<FunctorType>::moveTo(Scalar alpha)
253 {
254   x_alpha = x0 + alpha * p;
255   x_cache_key = alpha;
256 }
257 
258 template <typename FunctorType>
259 typename BFGS<FunctorType>::Scalar
slope()260 BFGS<FunctorType>::slope()
261 {
262   return (g_alpha.dot(p));
263 }
264 
265 template <typename FunctorType>
266 typename BFGS<FunctorType>::Scalar
applyF(Scalar alpha)267 BFGS<FunctorType>::applyF(Scalar alpha)
268 {
269   if (alpha == f_cache_key)
270     return f_alpha;
271   moveTo(alpha);
272   f_alpha = functor(x_alpha);
273   f_cache_key = alpha;
274   return (f_alpha);
275 }
276 
277 template <typename FunctorType>
278 typename BFGS<FunctorType>::Scalar
applyDF(Scalar alpha)279 BFGS<FunctorType>::applyDF(Scalar alpha)
280 {
281   if (alpha == df_cache_key)
282     return df_alpha;
283   moveTo(alpha);
284   if (alpha != g_cache_key) {
285     functor.df(x_alpha, g_alpha);
286     g_cache_key = alpha;
287   }
288   df_alpha = slope();
289   df_cache_key = alpha;
290   return (df_alpha);
291 }
292 
293 template <typename FunctorType>
294 void
applyFDF(Scalar alpha,Scalar & f,Scalar & df)295 BFGS<FunctorType>::applyFDF(Scalar alpha, Scalar& f, Scalar& df)
296 {
297   if (alpha == f_cache_key && alpha == df_cache_key) {
298     f = f_alpha;
299     df = df_alpha;
300     return;
301   }
302 
303   if (alpha == f_cache_key || alpha == df_cache_key) {
304     f = applyF(alpha);
305     df = applyDF(alpha);
306     return;
307   }
308 
309   moveTo(alpha);
310   functor.fdf(x_alpha, f_alpha, g_alpha);
311   f_cache_key = alpha;
312   g_cache_key = alpha;
313   df_alpha = slope();
314   df_cache_key = alpha;
315   f = f_alpha;
316   df = df_alpha;
317 }
318 
319 template <typename FunctorType>
320 void
updatePosition(Scalar alpha,FVectorType & x,Scalar & f,FVectorType & g)321 BFGS<FunctorType>::updatePosition(Scalar alpha,
322                                   FVectorType& x,
323                                   Scalar& f,
324                                   FVectorType& g)
325 {
326   {
327     Scalar f_alpha, df_alpha;
328     applyFDF(alpha, f_alpha, df_alpha);
329   };
330 
331   f = f_alpha;
332   x = x_alpha;
333   g = g_alpha;
334 }
335 
336 template <typename FunctorType>
337 void
changeDirection()338 BFGS<FunctorType>::changeDirection()
339 {
340   x_alpha = x0;
341   x_cache_key = 0.0;
342   f_cache_key = 0.0;
343   g_alpha = g0;
344   g_cache_key = 0.0;
345   df_alpha = slope();
346   df_cache_key = 0.0;
347 }
348 
349 template <typename FunctorType>
350 BFGSSpace::Status
minimize(FVectorType & x)351 BFGS<FunctorType>::minimize(FVectorType& x)
352 {
353   BFGSSpace::Status status = minimizeInit(x);
354   do {
355     status = minimizeOneStep(x);
356     iter++;
357   } while (status == BFGSSpace::Success && iter < parameters.max_iters);
358   return status;
359 }
360 
361 template <typename FunctorType>
362 BFGSSpace::Status
minimizeInit(FVectorType & x)363 BFGS<FunctorType>::minimizeInit(FVectorType& x)
364 {
365   iter = 0;
366   delta_f = 0;
367   dx.setZero();
368   functor.fdf(x, f, gradient);
369   x0 = x;
370   g0 = gradient;
371   g0norm = g0.norm();
372   p = gradient * -1 / g0norm;
373   pnorm = p.norm();
374   fp0 = -g0norm;
375 
376   {
377     x_alpha = x0;
378     x_cache_key = 0;
379 
380     f_alpha = f;
381     f_cache_key = 0;
382 
383     g_alpha = g0;
384     g_cache_key = 0;
385 
386     df_alpha = slope();
387     df_cache_key = 0;
388   }
389 
390   return BFGSSpace::NotStarted;
391 }
392 
393 template <typename FunctorType>
394 BFGSSpace::Status
minimizeOneStep(FVectorType & x)395 BFGS<FunctorType>::minimizeOneStep(FVectorType& x)
396 {
397   Scalar alpha = 0.0, alpha1;
398   Scalar f0 = f;
399   if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0) {
400     dx.setZero();
401     return BFGSSpace::NoProgress;
402   }
403 
404   if (delta_f < 0) {
405     Scalar del =
406         std::max(-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * std::abs(f0));
407     alpha1 = std::min(1.0, 2.0 * del / (-fp0));
408   }
409   else
410     alpha1 = std::abs(parameters.step_size);
411 
412   BFGSSpace::Status status = lineSearch(parameters.rho,
413                                         parameters.sigma,
414                                         parameters.tau1,
415                                         parameters.tau2,
416                                         parameters.tau3,
417                                         parameters.order,
418                                         alpha1,
419                                         alpha);
420 
421   if (status != BFGSSpace::Success)
422     return status;
423 
424   updatePosition(alpha, x, f, gradient);
425 
426   delta_f = f - f0;
427 
428   /* Choose a new direction for the next step */
429   {
430     /* This is the BFGS update: */
431     /* p' = g1 - A dx - B dg */
432     /* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */
433     /* B = dx.g/dx.dg */
434 
435     Scalar dxg, dgg, dxdg, dgnorm, A, B;
436 
437     /* dx0 = x - x0 */
438     dx0 = x - x0;
439     dx = dx0; /* keep a copy */
440 
441     /* dg0 = g - g0 */
442     dg0 = gradient - g0;
443     dxg = dx0.dot(gradient);
444     dgg = dg0.dot(gradient);
445     dxdg = dx0.dot(dg0);
446     dgnorm = dg0.norm();
447 
448     if (dxdg != 0) {
449       B = dxg / dxdg;
450       A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
451     }
452     else {
453       B = 0;
454       A = 0;
455     }
456 
457     p = -A * dx0;
458     p += gradient;
459     p += -B * dg0;
460   }
461 
462   g0 = gradient;
463   x0 = x;
464   g0norm = g0.norm();
465   pnorm = p.norm();
466 
467   Scalar dir = ((p.dot(gradient)) > 0) ? -1.0 : 1.0;
468   p *= dir / pnorm;
469   pnorm = p.norm();
470   fp0 = p.dot(g0);
471 
472   changeDirection();
473   return BFGSSpace::Success;
474 }
475 
476 template <typename FunctorType>
477 typename BFGSSpace::Status
testGradient()478 BFGS<FunctorType>::testGradient()
479 {
480   return functor.checkGradient(gradient);
481 }
482 
483 template <typename FunctorType>
484 typename BFGS<FunctorType>::Scalar
interpolate(Scalar a,Scalar fa,Scalar fpa,Scalar b,Scalar fb,Scalar fpb,Scalar xmin,Scalar xmax,int order)485 BFGS<FunctorType>::interpolate(Scalar a,
486                                Scalar fa,
487                                Scalar fpa,
488                                Scalar b,
489                                Scalar fb,
490                                Scalar fpb,
491                                Scalar xmin,
492                                Scalar xmax,
493                                int order)
494 {
495   /* Map [a,b] to [0,1] */
496   Scalar y, alpha, ymin, ymax, fmin;
497 
498   ymin = (xmin - a) / (b - a);
499   ymax = (xmax - a) / (b - a);
500 
501   // Ensure ymin <= ymax
502   if (ymin > ymax) {
503     Scalar tmp = ymin;
504     ymin = ymax;
505     ymax = tmp;
506   };
507 
508   if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits<Scalar>::infinity()) {
509     fpa = fpa * (b - a);
510     fpb = fpb * (b - a);
511 
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;
515     Scalar y0, y1;
516     Eigen::Matrix<Scalar, 4, 1> coefficients;
517     coefficients << c0, c1, c2, c3;
518 
519     y = ymin;
520     // Evaluate the cubic polyinomial at ymin;
521     fmin = Eigen::poly_eval(coefficients, ymin);
522     checkExtremum(coefficients, ymax, y, fmin);
523     {
524       // Solve quadratic polynomial for the derivate
525       Eigen::Matrix<Scalar, 3, 1> coefficients2;
526       coefficients2 << c1, 2 * c2, 3 * c3;
527       bool real_roots;
528       Eigen::PolynomialSolver<Scalar, 2> solver(coefficients2, real_roots);
529       if (real_roots) {
530         if ((solver.roots()).size() == 2) /* found 2 roots */
531         {
532           y0 = std::real(solver.roots()[0]);
533           y1 = std::real(solver.roots()[1]);
534           if (y0 > y1) {
535             Scalar tmp(y0);
536             y0 = y1;
537             y1 = tmp;
538           }
539           if (y0 > ymin && y0 < ymax)
540             checkExtremum(coefficients, y0, y, fmin);
541           if (y1 > ymin && y1 < ymax)
542             checkExtremum(coefficients, y1, y, fmin);
543         }
544         else if ((solver.roots()).size() == 1) /* found 1 root */
545         {
546           y0 = std::real(solver.roots()[0]);
547           if (y0 > ymin && y0 < ymax)
548             checkExtremum(coefficients, y0, y, fmin);
549         }
550       }
551     }
552   }
553   else {
554     fpa = fpa * (b - a);
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); /* curvature */
558     y = ymin;
559     fmin = fl;
560 
561     if (fh < fmin) {
562       y = ymax;
563       fmin = fh;
564     }
565 
566     if (c > a) /* positive curvature required for a minimum */
567     {
568       Scalar z = -fpa / c; /* location of minimum */
569       if (z > ymin && z < ymax) {
570         Scalar f = fa + z * (fpa + z * (fb - fa - fpa));
571         if (f < fmin) {
572           y = z;
573           fmin = f;
574         };
575       }
576     }
577   }
578 
579   alpha = a + y * (b - a);
580   return alpha;
581 }
582 
583 template <typename FunctorType>
584 BFGSSpace::Status
lineSearch(Scalar rho,Scalar sigma,Scalar tau1,Scalar tau2,Scalar tau3,int order,Scalar alpha1,Scalar & alpha_new)585 BFGS<FunctorType>::lineSearch(Scalar rho,
586                               Scalar sigma,
587                               Scalar tau1,
588                               Scalar tau2,
589                               Scalar tau3,
590                               int order,
591                               Scalar alpha1,
592                               Scalar& alpha_new)
593 {
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;
597   Index i = 0;
598 
599   applyFDF(0.0, f0, fp0);
600 
601   falpha_prev = f0;
602   fpalpha_prev = fp0;
603 
604   /* Avoid uninitialized variables morning */
605   a = 0.0;
606   b = alpha;
607   fa = f0;
608   fb = 0.0;
609   fpa = fp0;
610   fpb = 0.0;
611 
612   /* Begin bracketing */
613 
614   while (i++ < parameters.bracket_iters) {
615     falpha = applyF(alpha);
616 
617     if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev) {
618       a = alpha_prev;
619       fa = falpha_prev;
620       fpa = fpalpha_prev;
621       b = alpha;
622       fb = falpha;
623       fpb = std::numeric_limits<Scalar>::quiet_NaN();
624       break;
625     }
626 
627     fpalpha = applyDF(alpha);
628 
629     /* Fletcher's sigma test */
630     if (std::abs(fpalpha) <= -sigma * fp0) {
631       alpha_new = alpha;
632       return BFGSSpace::Success;
633     }
634 
635     if (fpalpha >= 0) {
636       a = alpha;
637       fa = falpha;
638       fpa = fpalpha;
639       b = alpha_prev;
640       fb = falpha_prev;
641       fpb = fpalpha_prev;
642       break; /* goto sectioning */
643     }
644 
645     delta = alpha - alpha_prev;
646 
647     {
648       Scalar lower = alpha + delta;
649       Scalar upper = alpha + tau1 * delta;
650 
651       alpha_next = interpolate(alpha_prev,
652                                falpha_prev,
653                                fpalpha_prev,
654                                alpha,
655                                falpha,
656                                fpalpha,
657                                lower,
658                                upper,
659                                order);
660     }
661 
662     alpha_prev = alpha;
663     falpha_prev = falpha;
664     fpalpha_prev = fpalpha;
665     alpha = alpha_next;
666   }
667   /*  Sectioning of bracket [a,b] */
668   while (i++ < parameters.section_iters) {
669     delta = b - a;
670 
671     {
672       Scalar lower = a + tau2 * delta;
673       Scalar upper = b - tau3 * delta;
674 
675       alpha = interpolate(a, fa, fpa, b, fb, fpb, lower, upper, order);
676     }
677     falpha = applyF(alpha);
678     if ((a - alpha) * fpa <= std::numeric_limits<Scalar>::epsilon()) {
679       /* roundoff prevents progress */
680       return BFGSSpace::NoProgress;
681     };
682 
683     if (falpha > f0 + rho * alpha * fp0 || falpha >= fa) {
684       /*  a_next = a; */
685       b = alpha;
686       fb = falpha;
687       fpb = std::numeric_limits<Scalar>::quiet_NaN();
688     }
689     else {
690       fpalpha = applyDF(alpha);
691 
692       if (std::abs(fpalpha) <= -sigma * fp0) {
693         alpha_new = alpha;
694         return BFGSSpace::Success; /* terminate */
695       }
696 
697       if (((b - a) >= 0 && fpalpha >= 0) || ((b - a) <= 0 && fpalpha <= 0)) {
698         b = a;
699         fb = fa;
700         fpb = fpa;
701         a = alpha;
702         fa = falpha;
703         fpa = fpalpha;
704       }
705       else {
706         a = alpha;
707         fa = falpha;
708         fpa = fpalpha;
709       }
710     }
711   }
712   return BFGSSpace::Success;
713 }
714