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