SLAMflex SE  0.1.0
SLAMflex provides detection and tracking of dominant planes for smartphone devices. This plane can then be used to show AR content relative to the plane orientation. The detection of plane is performed in the field of view of the smartphone camera. In subsequent frames it is tracked. The interface returns the plane position and orientation.
brent.h
Go to the documentation of this file.
1 #ifndef TOON_BRENT_H
2 #define TOON_BRENT_H
3 #include "TooN.h"
4 #include "helpers.h"
5 #include <limits>
6 #include <cmath>
7 #include <cstdlib>
8 #include <iomanip>
9 
10 
11 namespace TooN
12 {
13  using std::numeric_limits;
14 
29  template<class Functor, class Precision> Vector<2, Precision> brent_line_search(Precision a, Precision x, Precision b, Precision fx, const Functor& func, int maxiterations, Precision tolerance = sqrt(numeric_limits<Precision>::epsilon()), Precision epsilon = numeric_limits<Precision>::epsilon())
30  {
31  using std::min;
32  using std::max;
33 
34  using std::abs;
35 
36  //The golden ratio:
37  const Precision g = (3.0 - sqrt(5))/2;
38 
39  //The following points are tracked by the algorithm:
40  //a, b bracket the interval
41  // x is the best value so far
42  // w second best point so far
43  // v third best point so far
44  // These may not be unique.
45 
46  //The following points are used during iteration
47  // u the point currently being evaluated
48  // xm (a+b)/2
49 
50  //The updates are tracked as:
51  //e is the distance moved last step, or current if golden section is used
52  //d is the point moved in the current step
53 
54  Precision w=x, v=x, fw=fx, fv=fx;
55 
56  Precision d=0, e=0;
57  int i=0;
58 
59  while(abs(b-a) > (abs(a) + abs(b)) * tolerance + epsilon && i < maxiterations)
60  {
61  i++;
62  //The midpoint of the bracket
63  const Precision xm = (a+b)/2;
64 
65  //Per-iteration tolerance
66  const Precision tol1 = abs(x)*tolerance + epsilon;
67 
68  //If we recently had an unhelpful step, then do
69  //not attempt a parabolic fit. This prevents bad parabolic
70  //fits spoiling the convergence. Also, do not attempt to fit if
71  //there is not yet enough unique information in x, w, v.
72  if(abs(e) > tol1 && w != v)
73  {
74  //Attempt a parabolic through the best 3 points. The pdata is shifted
75  //so that x = 0 and f(x) = 0. The remaining parameters are:
76  //
77  // xw = w' = w-x
78  // fxw = f'(w) = f(w) - f(x)
79  //
80  // etc:
81  const Precision fxw = fw - fx;
82  const Precision fxv = fv - fx;
83  const Precision xw = w-x;
84  const Precision xv = v-x;
85 
86  //The parabolic fit has only second and first order coefficients:
87  //const Precision c1 = (fxv*xw - fxw*xv) / (xw*xv*(xv-xw));
88  //const Precision c2 = (fxw*xv*xv - fxv*xw*xw) / (xw*xv*(xv-xw));
89 
90  //The minimum is at -.5*c2 / c1;
91  //
92  //This can be written as p/q where:
93  const Precision p = fxv*xw*xw - fxw*xv*xv;
94  const Precision q = 2*(fxv*xw - fxw*xv);
95 
96  //The minimum is at p/q. The minimum must lie within the bracket for it
97  //to be accepted.
98  // Also, we want the step to be smaller than half the old one. So:
99 
100  if(q == 0 || x + p/q < a || x+p/q > b || abs(p/q) > abs(e/2))
101  {
102  //Parabolic fit no good. Take a golden section step instead
103  //and reset d and e.
104  if(x > xm)
105  e = a-x;
106  else
107  e = b-x;
108 
109  d = g*e;
110  }
111  else
112  {
113  //Parabolic fit was good. Shift d and e
114  e = d;
115  d = p/q;
116  }
117  }
118  else
119  {
120  //Don't attempt a parabolic fit. Take a golden section step
121  //instead and reset d and e.
122  if(x > xm)
123  e = a-x;
124  else
125  e = b-x;
126 
127  d = g*e;
128  }
129 
130  const Precision u = x+d;
131  //Our one function evaluation per iteration
132  const Precision fu = func(u);
133 
134  if(fu < fx)
135  {
136  //U is the best known point.
137 
138  //Update the bracket
139  if(u > x)
140  a = x;
141  else
142  b = x;
143 
144  //Shift v, w, x
145  v=w; fv = fw;
146  w=x; fw = fx;
147  x=u; fx = fu;
148  }
149  else
150  {
151  //u is not the best known point. However, it is within the
152  //bracket.
153  if(u < x)
154  a = u;
155  else
156  b = u;
157 
158  if(fu <= fw || w == x)
159  {
160  //Here, u is the new second-best point
161  v = w; fv = fw;
162  w = u; fw = fu;
163  }
164  else if(fu <= fv || v==x || v == w)
165  {
166  //Here, u is the new third-best point.
167  v = u; fv = fu;
168  }
169  }
170  }
171 
172  return makeVector(x, fx);
173  }
174 }
175 #endif
Everything lives inside this namespace.
Definition: allocator.hh:48
Vector< 2, Precision > brent_line_search(Precision a, Precision x, Precision b, Precision fx, const Functor &func, int maxiterations, Precision tolerance=sqrt(numeric_limits< Precision >::epsilon()), Precision epsilon=numeric_limits< Precision >::epsilon())
Definition: brent.h:29
T abs(T t)
Definition: abs.h:30
Vector< 1 > makeVector(double x1)
Definition: make_vector.hh:4