bfgs.h
Go to the documentation of this file.
00001 #ifndef PCL_FOR_EIGEN_BFGS_H
00002 #define PCL_FOR_EIGEN_BFGS_H
00003 
00004 #if defined __GNUC__
00005 #  pragma GCC system_header 
00006 #endif
00007 
00008 #include <pcl/registration/eigen.h>
00009 
00010 namespace Eigen
00011 {
00012   template< typename _Scalar >
00013   class PolynomialSolver<_Scalar,2> : public PolynomialSolverBase<_Scalar,2>
00014   {
00015     public:
00016       typedef PolynomialSolverBase<_Scalar,2>    PS_Base;
00017       EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base )
00018         
00019     public:
00020 
00021       virtual ~PolynomialSolver () {}
00022 
00023       template< typename OtherPolynomial >
00024       inline PolynomialSolver( const OtherPolynomial& poly, bool& hasRealRoot )
00025       {
00026         compute( poly, hasRealRoot );
00027       }
00028       
00030       template< typename OtherPolynomial >
00031       void compute( const OtherPolynomial& poly, bool& hasRealRoot)
00032       {
00033         const Scalar ZERO(0);
00034         Scalar a2(2 * poly[2]);
00035         assert( ZERO != poly[poly.size()-1] );
00036         Scalar discriminant ((poly[1] * poly[1]) - (4 * poly[0] * poly[2]));
00037         if (ZERO < discriminant)
00038         {
00039           Scalar discriminant_root (std::sqrt (discriminant));
00040           m_roots[0] = (-poly[1] - discriminant_root) / (a2) ;
00041           m_roots[1] = (-poly[1] + discriminant_root) / (a2) ;
00042           hasRealRoot = true;
00043         }
00044         else {
00045           if (ZERO == discriminant)
00046           {
00047             m_roots.resize (1);
00048             m_roots[0] = -poly[1] / a2;
00049             hasRealRoot = true;
00050           }
00051           else
00052           {
00053             Scalar discriminant_root (std::sqrt (-discriminant));
00054             m_roots[0] = RootType (-poly[1] / a2, -discriminant_root / a2);
00055             m_roots[1] = RootType (-poly[1] / a2,  discriminant_root / a2);
00056             hasRealRoot = false;
00057           }
00058         }
00059       }
00060       
00061       template< typename OtherPolynomial >
00062       void compute( const OtherPolynomial& poly)
00063       {
00064         bool hasRealRoot;
00065         compute(poly, hasRealRoot);
00066       }
00067 
00068     protected:
00069       using                   PS_Base::m_roots;
00070   };
00071 }
00072 
00073 template<typename _Scalar, int NX=Eigen::Dynamic>
00074 struct BFGSDummyFunctor
00075 {
00076   typedef _Scalar Scalar;
00077   enum { InputsAtCompileTime = NX };
00078   typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> VectorType;
00079 
00080   const int m_inputs;
00081 
00082   BFGSDummyFunctor() : m_inputs(InputsAtCompileTime) {}
00083   BFGSDummyFunctor(int inputs) : m_inputs(inputs) {}
00084 
00085   virtual ~BFGSDummyFunctor() {}
00086   int inputs() const { return m_inputs; }
00087 
00088   virtual double operator() (const VectorType &x) = 0;
00089   virtual void  df(const VectorType &x, VectorType &df) = 0;
00090   virtual void fdf(const VectorType &x, Scalar &f, VectorType &df) = 0;
00091 };
00092 
00093 namespace BFGSSpace {
00094   enum Status {
00095     NegativeGradientEpsilon = -3,
00096     NotStarted = -2,
00097     Running = -1,
00098     Success = 0,
00099     NoProgress = 1
00100   };
00101 }
00102 
00113 template<typename FunctorType>
00114 class BFGS
00115 {
00116 public:
00117   typedef typename FunctorType::Scalar Scalar;
00118   typedef typename FunctorType::VectorType FVectorType;
00119 
00120   BFGS(FunctorType &_functor) 
00121       : pnorm(0), g0norm(0), iter(-1), functor(_functor) {  }
00122 
00123   typedef Eigen::DenseIndex Index;
00124 
00125   struct Parameters {
00126     Parameters()
00127     : max_iters(400)
00128       , bracket_iters(100)
00129       , section_iters(100)
00130       , rho(0.01)
00131       , sigma(0.01)
00132       , tau1(9)
00133       , tau2(0.05)
00134       , tau3(0.5)
00135       , step_size(1)
00136       , order(3) {}
00137     Index max_iters;   // maximum number of function evaluation
00138     Index bracket_iters;
00139     Index section_iters;
00140     Scalar rho;
00141     Scalar sigma;
00142     Scalar tau1;
00143     Scalar tau2;
00144     Scalar tau3;
00145     Scalar step_size;
00146     Index order;
00147   };
00148 
00149   BFGSSpace::Status minimize(FVectorType &x);
00150   BFGSSpace::Status minimizeInit(FVectorType &x);
00151   BFGSSpace::Status minimizeOneStep(FVectorType &x);
00152   BFGSSpace::Status testGradient(Scalar epsilon);
00153   void resetParameters(void) { parameters = Parameters(); }
00154   
00155   Parameters parameters;
00156   Scalar f;
00157   FVectorType gradient;
00158 private:
00159   
00160   BFGS& operator=(const BFGS&);
00161   BFGSSpace::Status lineSearch (Scalar rho, Scalar sigma, 
00162                                 Scalar tau1, Scalar tau2, Scalar tau3,
00163                                 int order, Scalar alpha1, Scalar &alpha_new);
00164   Scalar interpolate (Scalar a, Scalar fa, Scalar fpa,
00165                       Scalar b, Scalar fb, Scalar fpb, Scalar xmin, Scalar xmax,
00166                       int order);  
00167   void checkExtremum (const Eigen::Matrix<Scalar, 4, 1>& coefficients, Scalar x, Scalar& xmin, Scalar& fmin);
00168   void moveTo (Scalar alpha);
00169   Scalar slope ();
00170   Scalar applyF (Scalar alpha);
00171   Scalar applyDF (Scalar alpha);
00172   void applyFDF (Scalar alpha, Scalar &f, Scalar &df);
00173   void updatePosition (Scalar alpha, FVectorType& x, Scalar& f, FVectorType& g);
00174   void changeDirection ();
00175   
00176   Scalar delta_f, fp0;
00177   FVectorType x0, dx0, dg0, g0, dx, p;
00178   Scalar pnorm, g0norm;
00179 
00180   Scalar f_alpha;
00181   Scalar df_alpha;
00182   FVectorType x_alpha;
00183   FVectorType g_alpha;
00184   
00185   // cache "keys"
00186   Scalar f_cache_key;
00187   Scalar df_cache_key;
00188   Scalar x_cache_key;
00189   Scalar g_cache_key;
00190 
00191   Index iter;
00192   FunctorType &functor;
00193 };
00194 
00195 
00196 template<typename FunctorType> void
00197 BFGS<FunctorType>::checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients, Scalar x, Scalar& xmin, Scalar& fmin)
00198 {
00199   Scalar y = Eigen::poly_eval(coefficients, x);
00200   if(y < fmin) { xmin = x; fmin = y; }
00201 }
00202 
00203 template<typename FunctorType> void
00204 BFGS<FunctorType>::moveTo(Scalar alpha)
00205 {
00206   x_alpha = x0 + alpha * p;
00207   x_cache_key = alpha;
00208 }
00209 
00210 template<typename FunctorType> typename BFGS<FunctorType>::Scalar
00211 BFGS<FunctorType>::slope()
00212 {
00213   return (g_alpha.dot (p));
00214 }
00215 
00216 template<typename FunctorType> typename BFGS<FunctorType>::Scalar
00217 BFGS<FunctorType>::applyF(Scalar alpha)
00218 {
00219   if (alpha == f_cache_key) return f_alpha;
00220   moveTo (alpha);
00221   f_alpha = functor (x_alpha);
00222   f_cache_key = alpha;
00223   return (f_alpha);
00224 }
00225 
00226 template<typename FunctorType> typename BFGS<FunctorType>::Scalar
00227 BFGS<FunctorType>::applyDF(Scalar alpha)
00228 {
00229   if (alpha == df_cache_key) return df_alpha;
00230   moveTo (alpha);
00231   if(alpha != g_cache_key)
00232   {
00233     functor.df (x_alpha, g_alpha);
00234     g_cache_key = alpha;
00235   }
00236   df_alpha = slope ();
00237   df_cache_key = alpha;
00238   return (df_alpha);
00239 }
00240 
00241 template<typename FunctorType> void
00242 BFGS<FunctorType>::applyFDF(Scalar alpha, Scalar& f, Scalar& df)
00243 {
00244   if(alpha == f_cache_key && alpha == df_cache_key)
00245   {
00246     f = f_alpha;
00247     df = df_alpha;
00248     return;
00249   }
00250 
00251   if(alpha == f_cache_key || alpha == df_cache_key)
00252   {
00253     f = applyF (alpha);
00254     df = applyDF (alpha);
00255     return;
00256   }
00257 
00258   moveTo (alpha);
00259   functor.fdf (x_alpha, f_alpha, g_alpha);
00260   f_cache_key = alpha;
00261   g_cache_key = alpha;
00262   df_alpha = slope ();
00263   df_cache_key = alpha;
00264   f = f_alpha;
00265   df = df_alpha;
00266 }
00267 
00268 template<typename FunctorType> void
00269 BFGS<FunctorType>::updatePosition (Scalar alpha, FVectorType &x, Scalar &f, FVectorType &g)
00270 {
00271   { 
00272     Scalar f_alpha, df_alpha; 
00273     applyFDF (alpha, f_alpha, df_alpha); 
00274   } ;
00275 
00276   f = f_alpha;
00277   x = x_alpha;
00278   g = g_alpha;
00279 }  
00280 
00281 template<typename FunctorType> void
00282 BFGS<FunctorType>::changeDirection ()
00283 {
00284   x_alpha = x0;
00285   x_cache_key = 0.0;
00286   f_cache_key = 0.0;
00287   g_alpha = g0;
00288   g_cache_key = 0.0;
00289   df_alpha = slope ();
00290   df_cache_key = 0.0;
00291 }
00292 
00293 template<typename FunctorType> BFGSSpace::Status
00294 BFGS<FunctorType>::minimize(FVectorType  &x)
00295 {
00296   BFGSSpace::Status status = minimizeInit(x);
00297   do {
00298     status = minimizeOneStep(x);
00299     iter++;
00300   } while (status==BFGSSpace::Success && iter < parameters.max_iters);
00301   return status;
00302 }
00303 
00304 template<typename FunctorType> BFGSSpace::Status
00305 BFGS<FunctorType>::minimizeInit(FVectorType  &x)
00306 {
00307   iter = 0;
00308   delta_f = 0;
00309   dx.setZero ();
00310   functor.fdf(x, f, gradient);
00311   x0 = x;
00312   g0 = gradient;
00313   g0norm = g0.norm ();
00314   p = gradient * -1/g0norm;
00315   pnorm = p.norm ();
00316   fp0 = -g0norm;
00317 
00318   {
00319     x_alpha = x0; x_cache_key = 0;
00320     
00321     f_alpha = f; f_cache_key = 0;
00322     
00323     g_alpha = g0; g_cache_key = 0;
00324     
00325     df_alpha = slope (); df_cache_key = 0;
00326   }
00327 
00328   return BFGSSpace::NotStarted;
00329 }
00330 
00331 template<typename FunctorType> BFGSSpace::Status
00332 BFGS<FunctorType>::minimizeOneStep(FVectorType  &x)
00333 {
00334   Scalar alpha = 0.0, alpha1;
00335   Scalar f0 = f;
00336   if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0)
00337   {
00338     dx.setZero ();
00339     return BFGSSpace::NoProgress;
00340   }
00341 
00342   if (delta_f < 0)
00343   {
00344     Scalar del = std::max (-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * fabs(f0));
00345     alpha1 = std::min (1.0, 2.0 * del / (-fp0));
00346   }
00347   else
00348     alpha1 = fabs(parameters.step_size);
00349 
00350   BFGSSpace::Status status = lineSearch(parameters.rho, parameters.sigma, 
00351                                         parameters.tau1, parameters.tau2, parameters.tau3, 
00352                                         parameters.order, alpha1, alpha);
00353 
00354   if(status != BFGSSpace::Success)
00355     return status;
00356 
00357   updatePosition(alpha, x, f, gradient);
00358 
00359   delta_f = f - f0;
00360 
00361   /* Choose a new direction for the next step */
00362   {
00363     /* This is the BFGS update: */
00364     /* p' = g1 - A dx - B dg */
00365     /* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */
00366     /* B = dx.g/dx.dg */
00367 
00368     Scalar dxg, dgg, dxdg, dgnorm, A, B;
00369 
00370     /* dx0 = x - x0 */
00371     dx0 = x - x0;
00372     dx = dx0; /* keep a copy */
00373 
00374     /* dg0 = g - g0 */
00375     dg0 = gradient - g0;
00376     dxg = dx0.dot (gradient);
00377     dgg = dg0.dot (gradient);
00378     dxdg = dx0.dot (dg0);
00379     dgnorm = dg0.norm ();
00380 
00381     if (dxdg != 0)
00382     {
00383       B = dxg / dxdg;
00384       A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
00385     }
00386     else
00387     {
00388       B = 0;
00389       A = 0;
00390     }
00391 
00392     p = -A * dx0;
00393     p+= gradient;
00394     p+= -B * dg0 ;
00395   }
00396 
00397   g0 = gradient;
00398   x0 = x;
00399   g0norm = g0.norm ();
00400   pnorm = p.norm ();
00401   
00402   Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0;
00403   p*= dir / pnorm;
00404   pnorm = p.norm ();
00405   fp0 = p.dot (g0);
00406 
00407   changeDirection();
00408   return BFGSSpace::Success;
00409 }
00410 
00411 template<typename FunctorType> typename BFGSSpace::Status 
00412 BFGS<FunctorType>::testGradient(Scalar epsilon)
00413 {
00414   if(epsilon < 0)
00415     return BFGSSpace::NegativeGradientEpsilon;
00416   else
00417   {
00418     if(gradient.norm () < epsilon)
00419       return BFGSSpace::Success;
00420     else
00421       return BFGSSpace::Running;
00422   }
00423 }
00424 
00425 template<typename FunctorType> typename BFGS<FunctorType>::Scalar 
00426 BFGS<FunctorType>::interpolate (Scalar a, Scalar fa, Scalar fpa,
00427                                 Scalar b, Scalar fb, Scalar fpb, 
00428                                 Scalar xmin, Scalar xmax,
00429                                 int order)
00430 {
00431   /* Map [a,b] to [0,1] */
00432   Scalar y, alpha, ymin, ymax, fmin;
00433 
00434   ymin = (xmin - a) / (b - a);
00435   ymax = (xmax - a) / (b - a);
00436   
00437   // Ensure ymin <= ymax
00438   if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; };
00439 
00440   if (order > 2 && !(fpb != fpb) && fpb != std::numeric_limits<Scalar>::infinity ()) 
00441   {
00442     fpa = fpa * (b - a);
00443     fpb = fpb * (b - a);
00444 
00445     Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb;
00446     Scalar xi = fpa + fpb - 2 * (fb - fa);
00447     Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi;
00448     Scalar y0, y1;
00449     Eigen::Matrix<Scalar, 4, 1> coefficients;
00450     coefficients << c0, c1, c2, c3;
00451     
00452     y = ymin; 
00453     // Evaluate the cubic polyinomial at ymin;
00454     fmin = Eigen::poly_eval (coefficients, ymin);
00455     checkExtremum (coefficients, ymax, y, fmin);
00456     {
00457       // Solve quadratic polynomial for the derivate
00458       Eigen::Matrix<Scalar, 3, 1> coefficients2;
00459       coefficients2 << c1, 2 * c2, 3 * c3;
00460       bool real_roots;
00461       Eigen::PolynomialSolver<Scalar, 2> solver (coefficients2, real_roots);
00462       if(real_roots)
00463       {
00464         if ((solver.roots ()).size () == 2)  /* found 2 roots */
00465         {
00466           y0 = std::real (solver.roots () [0]);
00467           y1 = std::real (solver.roots () [1]);
00468           if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; }
00469           if (y0 > ymin && y0 < ymax) 
00470             checkExtremum (coefficients, y0, y, fmin);
00471           if (y1 > ymin && y1 < ymax) 
00472             checkExtremum (coefficients, y1, y, fmin);
00473         }
00474         else if ((solver.roots ()).size () == 1)  /* found 1 root */
00475         {
00476           y0 = std::real (solver.roots () [0]);
00477           if (y0 > ymin && y0 < ymax) 
00478             checkExtremum (coefficients, y0, y, fmin);
00479         }
00480       }
00481     }
00482   } 
00483   else 
00484   {
00485     fpa = fpa * (b - a);
00486     Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa));
00487     Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa));
00488     Scalar c = 2 * (fb - fa - fpa);       /* curvature */
00489     y = ymin; fmin = fl;
00490     
00491     if (fh < fmin) { y = ymax; fmin = fh; } 
00492     
00493     if (c > a)  /* positive curvature required for a minimum */
00494     {
00495       Scalar z = -fpa / c;      /* location of minimum */
00496       if (z > ymin && z < ymax) {
00497         Scalar f = fa + z*(fpa + z*(fb - fa -fpa));
00498         if (f < fmin) { y = z; fmin = f; };
00499       }
00500     }
00501   }
00502   
00503   alpha = a + y * (b - a);
00504   return alpha;
00505 }
00506 
00507 template<typename FunctorType> BFGSSpace::Status 
00508 BFGS<FunctorType>::lineSearch(Scalar rho, Scalar sigma, 
00509                               Scalar tau1, Scalar tau2, Scalar tau3,
00510                               int order, Scalar alpha1, Scalar &alpha_new)
00511 {
00512   Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next;
00513   Scalar alpha = alpha1, alpha_prev = 0.0;
00514   Scalar a, b, fa, fb, fpa, fpb;
00515   Index i = 0;
00516 
00517   applyFDF (0.0, f0, fp0);
00518 
00519   falpha_prev = f0;
00520   fpalpha_prev = fp0;
00521 
00522   /* Avoid uninitialized variables morning */
00523   a = 0.0; b = alpha;
00524   fa = f0; fb = 0.0;
00525   fpa = fp0; fpb = 0.0;
00526 
00527   /* Begin bracketing */  
00528 
00529   while (i++ < parameters.bracket_iters)
00530   {
00531     falpha = applyF (alpha);
00532 
00533     if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev)
00534     {
00535       a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev;
00536       b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
00537       break;
00538     } 
00539 
00540     fpalpha = applyDF (alpha);
00541 
00542     /* Fletcher's sigma test */
00543     if (fabs (fpalpha) <= -sigma * fp0)
00544     {
00545       alpha_new = alpha;
00546       return BFGSSpace::Success;
00547     }
00548 
00549     if (fpalpha >= 0)
00550     {
00551       a = alpha; fa = falpha; fpa = fpalpha;
00552       b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev;
00553       break;                /* goto sectioning */
00554     }
00555 
00556     delta = alpha - alpha_prev;
00557 
00558     {
00559       Scalar lower = alpha + delta;
00560       Scalar upper = alpha + tau1 * delta;
00561 
00562       alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev,
00563                                 alpha, falpha, fpalpha, lower, upper, order);
00564 
00565     }
00566 
00567     alpha_prev = alpha;
00568     falpha_prev = falpha;
00569     fpalpha_prev = fpalpha;
00570     alpha = alpha_next;
00571   }
00572   /*  Sectioning of bracket [a,b] */
00573   while (i++ < parameters.section_iters)
00574   {
00575     delta = b - a;
00576       
00577     {
00578       Scalar lower = a + tau2 * delta;
00579       Scalar upper = b - tau3 * delta;
00580         
00581       alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order);
00582     }
00583     falpha = applyF (alpha);
00584     if ((a-alpha)*fpa <= std::numeric_limits<Scalar>::epsilon ()) {
00585       /* roundoff prevents progress */
00586       return BFGSSpace::NoProgress;
00587     };
00588 
00589     if (falpha > f0 + rho * alpha * fp0 || falpha >= fa)
00590     {
00591       /*  a_next = a; */
00592       b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN ();
00593     }
00594     else
00595     {
00596       fpalpha = applyDF (alpha);
00597           
00598       if (fabs(fpalpha) <= -sigma * fp0)
00599       {
00600         alpha_new = alpha;
00601         return BFGSSpace::Success;  /* terminate */
00602       }
00603           
00604       if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0))
00605       {
00606         b = a; fb = fa; fpb = fpa;
00607         a = alpha; fa = falpha; fpa = fpalpha;
00608       }
00609       else
00610       {
00611         a = alpha; fa = falpha; fpa = fpalpha;
00612       }
00613     }
00614   }
00615   return BFGSSpace::Success;
00616 }
00617 #endif // PCL_FOR_EIGEN_BFGS_H
00618 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:22:35