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