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