core/vnl/algo/vnl_bracket_minimum.cxx
Go to the documentation of this file.
00001 // This is core/vnl/algo/vnl_bracket_minimum.cxx
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005 //:
00006 // \file
00007 // \brief Function to bracket a minimum
00008 // \author Tim Cootes
00009 // \date   Feb 2007
00010 
00011 #include "vnl_bracket_minimum.h"
00012 #include <vnl/algo/vnl_fit_parabola.h>
00013 #include <vcl_cmath.h>
00014 #include <vcl_algorithm.h>
00015 // not used? #include <vcl_iostream.h>
00016 
00017 static const double GOLDEN_RATIO = 1.618033988749894848; // = 0.5*(vcl_sqrt(5)-1);
00018 static const double EPS   = 1e-7;  // Loose tolerance
00019 static const double EPSqr = 1e-14;
00020 inline void swap(double& a, double& b)
00021 {
00022   double x=a;
00023   a=b;
00024   b=x;
00025 }
00026 
00027 class vnl_bm_func
00028 {
00029   vnl_vector<double> v;
00030   vnl_cost_function* f;
00031  public:
00032   vnl_bm_func(vnl_cost_function& fn) { f=&fn; v.set_size(1); }
00033   double operator()(double x) { v[0]=x; return f->f(v); }
00034 };
00035 
00036 //: Given initial values a and b, find bracket a<b<c s.t. f(a)>f(b)<f(c)
00037 //  Final function values at a,b,c stored in fa,fb,fc
00038 void vnl_bracket_minimum(vnl_cost_function& fn,
00039                          double& a, double& b, double& c,
00040                          double& fa, double& fb, double& fc)
00041 {
00042   // Set up object to evaluate function
00043   // Note that fn takes a vector input - f converts a scalar to a vector
00044   vnl_bm_func f(fn);
00045 
00046   if (b==a) b=a+1.0;
00047   fa = f(a);
00048   fb = f(b);
00049 
00050   // Arrange that fb<=fa
00051   if (fb>fa)
00052   {
00053     swap(a,b); swap(fa,fb);
00054   }
00055 
00056   // Initial guess at c
00057   c = b+ GOLDEN_RATIO*(b-a);
00058   fc = f(c);
00059 
00060   while (fc<fb)  // Keep stepping until we go uphill again
00061   {
00062     // Use parabolic interpolation to estimate position of centre
00063     double p,q;
00064     vnl_fit_parabola(a,b,c,fa,fb,fc,p,q);
00065 
00066     // Ensure q not within EPSqr of zero
00067     if (q>=0 && q<EPSqr) q=EPSqr;
00068     else if (q<0 && q+EPSqr>0) q=-1.0*EPSqr;
00069 
00070     // Estimate of centre of parabolic fit - ie minimum
00071     // For true quadratic function, minima is at b+p/q
00072     double du = p/q;
00073 
00074     double tol = EPS*(1.0+vcl_max(vcl_fabs(b),vcl_fabs(c)));
00075 
00076     // Don't evaluate too close to b
00077     if (du>=0 && du<tol)       du=tol;
00078     else if (du<0 && du+tol>0) du=-1.0*tol;
00079 
00080     double u = b + du;
00081 
00082     // Don't evaluate too close to c
00083     if ((u-c)<tol && (u-c)>=0)        u+=tol;  // u>c by small amount
00084     else if ((c-u)<tol && (c-u)>=0)   u-=tol;  // u<c by small amount
00085 
00086     double u_limit = b + 100*(c-b);  // Some way along the line
00087     double fu=0.0;
00088 
00089     if ((u-b)*(c-u)>0.0)  // u in range (b,c), allowing for c<b
00090     {
00091       fu = f(u);
00092       if (fu<fc)
00093       {
00094         // Bracket is (b,u,c)
00095         a=b; fa=fb;  b=u; fb=fu;
00096         // Ensure a<b<c
00097         if (a>c) { swap(a,c); swap(fa,fc); }
00098         return;
00099       }
00100       else if (fu>fb)
00101       {
00102         // Bracket is (a,b,u)
00103         c=u; fc=fu;
00104         // Ensure a<b<c
00105         if (a>c) { swap(a,c); swap(fa,fc); }
00106         return;
00107       }
00108       // The predicted point is unhelpful, so try a default step
00109       u = c+GOLDEN_RATIO*(c-b);
00110       fu = f(u);
00111     }
00112     else if ((u-c)*(u_limit-u)>0.0)  // u in range (c,u_limit)
00113     {
00114       fu = f(u);
00115       if (fu>fc)
00116       {
00117         // Bracket is (b,c,u)
00118         a=b; fa=fb;  b=c; fb=fc; c=u; fc=fu;
00119         // Ensure a<b<c
00120         if (a>c) { swap(a,c); swap(fa,fc); }
00121         return;
00122       }
00123     }
00124     else if ((u_limit-c)*(u-u_limit)>=0) // u is beyond u_limit
00125     {
00126       u=u_limit;
00127       fu=f(u);
00128     }
00129     else  // u is somewhere else
00130     {
00131       // Next guess is at u
00132       u = c+GOLDEN_RATIO*(c-b);
00133       fu = f(u);
00134     }
00135 
00136     // Move bracket
00137     a=b;   b=c;    c=u;
00138     fa=fb; fb=fc; fc=fu;
00139   }
00140 
00141   // Ensure a<b<c
00142   if (a>c)
00143   {
00144     swap(a,c); swap(fa,fc);
00145   }
00146 }
00147