Blender  V3.3
dogleg.h
Go to the documentation of this file.
1 // Copyright (c) 2007, 2008, 2009 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20 //
21 // A simple implementation of Powell's dogleg nonlinear minimization.
22 //
23 // [1] K. Madsen, H. Nielsen, O. Tingleoff. Methods for Non-linear Least
24 // Squares Problems.
25 // http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
26 //
27 // TODO(keir): Cite the Lourakis' dogleg paper.
28 
29 #ifndef LIBMV_NUMERIC_DOGLEG_H
30 #define LIBMV_NUMERIC_DOGLEG_H
31 
32 #include <cmath>
33 #include <cstdio>
34 
35 #include "libmv/logging/logging.h"
37 #include "libmv/numeric/numeric.h"
38 
39 namespace libmv {
40 
41 template <typename Function,
42  typename Jacobian = NumericJacobian<Function>,
43  typename Solver = Eigen::PartialPivLU<
44  Matrix<typename Function::FMatrixType::RealScalar,
45  Function::XMatrixType::RowsAtCompileTime,
46  Function::XMatrixType::RowsAtCompileTime>>>
47 class Dogleg {
48  public:
49  typedef typename Function::XMatrixType::RealScalar Scalar;
50  typedef typename Function::FMatrixType FVec;
51  typedef typename Function::XMatrixType Parameters;
52  typedef Matrix<typename Function::FMatrixType::RealScalar,
53  Function::FMatrixType::RowsAtCompileTime,
54  Function::XMatrixType::RowsAtCompileTime>
56  typedef Matrix<typename JMatrixType::RealScalar,
57  JMatrixType::ColsAtCompileTime,
58  JMatrixType::ColsAtCompileTime>
60 
61  enum Status {
63  GRADIENT_TOO_SMALL, // eps > max(J'*f(x))
64  RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / ||x||
65  TRUST_REGION_TOO_SMALL, // eps > radius / ||x||
66  ERROR_TOO_SMALL, // eps > ||f(x)||
68  };
69 
70  enum Step {
74  };
75 
76  Dogleg(const Function& f) : f_(f), df_(f) {}
77 
80  : gradient_threshold(1e-16),
82  error_threshold(1e-16),
84  max_iterations(500) {}
85  Scalar gradient_threshold; // eps > max(J'*f(x))
86  Scalar relative_step_threshold; // eps > ||dx|| / ||x||
87  Scalar error_threshold; // eps > ||f(x)||
88  Scalar initial_trust_radius; // Initial u for solving normal equations.
89  int max_iterations; // Maximum number of solver iterations.
90  };
91 
92  struct Results {
93  Scalar error_magnitude; // ||f(x)||
94  Scalar gradient_magnitude; // ||J'f(x)||
97  };
98 
100  const SolverParameters& params,
101  JMatrixType* J,
102  AMatrixType* A,
103  FVec* error,
104  Parameters* g) {
105  *J = df_(x);
106  // TODO(keir): In the case of m = n, avoid computing A and just do J^-1
107  // directly.
108  *A = (*J).transpose() * (*J);
109  *error = f_(x);
110  *g = (*J).transpose() * *error;
111  if (g->array().abs().maxCoeff() < params.gradient_threshold) {
112  return GRADIENT_TOO_SMALL;
113  } else if (error->array().abs().maxCoeff() < params.error_threshold) {
114  return ERROR_TOO_SMALL;
115  }
116  return RUNNING;
117  }
118 
120  const Parameters& dx_gn,
121  Scalar radius,
122  Scalar alpha,
123  Parameters* dx_dl,
124  Scalar* beta) {
125  Parameters a, b_minus_a;
126  // Solve for Dogleg step dx_dl.
127  if (dx_gn.norm() < radius) {
128  *dx_dl = dx_gn;
129  return GAUSS_NEWTON;
130 
131  } else if (alpha * dx_sd.norm() > radius) {
132  *dx_dl = (radius / dx_sd.norm()) * dx_sd;
133  return STEEPEST_DESCENT;
134 
135  } else {
136  Parameters a = alpha * dx_sd;
137  const Parameters& b = dx_gn;
138  b_minus_a = a - b;
139  Scalar Mbma2 = b_minus_a.squaredNorm();
140  Scalar Ma2 = a.squaredNorm();
141  Scalar c = a.dot(b_minus_a);
142  Scalar radius2 = radius * radius;
143  if (c <= 0) {
144  *beta = (-c + sqrt(c * c + Mbma2 * (radius2 - Ma2))) / (Mbma2);
145  } else {
146  *beta = (radius2 - Ma2) / (c + sqrt(c * c + Mbma2 * (radius2 - Ma2)));
147  }
148  *dx_dl = alpha * dx_sd + (*beta) * (dx_gn - alpha * dx_sd);
149  return DOGLEG;
150  }
151  }
152 
155  return minimize(params, x_and_min);
156  }
157 
159  Parameters& x = *x_and_min;
160  JMatrixType J;
161  AMatrixType A;
162  FVec error;
163  Parameters g;
164 
165  Results results;
166  results.status = Update(x, params, &J, &A, &error, &g);
167 
168  Scalar radius = params.initial_trust_radius;
169  bool x_updated = true;
170 
171  Parameters x_new;
172  Parameters dx_sd; // Steepest descent step.
173  Parameters dx_dl; // Dogleg step.
174  Parameters dx_gn; // Gauss-Newton step.
175  printf("iteration ||f(x)|| max(g) radius\n");
176  int i = 0;
177  for (; results.status == RUNNING && i < params.max_iterations; ++i) {
178  printf("%9d %12g %12g %12g",
179  i,
180  f_(x).norm(),
181  g.array().abs().maxCoeff(),
182  radius);
183 
184  // LG << "iteration: " << i;
185  // LG << "||f(x)||: " << f_(x).norm();
186  // LG << "max(g): " << g.cwise().abs().maxCoeff();
187  // LG << "radius: " << radius;
188  // Eqn 3.19 from [1]
189  Scalar alpha = g.squaredNorm() / (J * g).squaredNorm();
190 
191  // Solve for steepest descent direction dx_sd.
192  dx_sd = -g;
193 
194  // Solve for Gauss-Newton direction dx_gn.
195  if (x_updated) {
196  // TODO(keir): See Appendix B of [1] for discussion of when A is
197  // singular and there are many solutions. Solving that involves the SVD
198  // and is slower, but should still work.
199  Solver solver(A);
200  dx_gn = solver.solve(-g);
201  if (!(A * dx_gn).isApprox(-g)) {
202  LOG(ERROR) << "Failed to solve normal eqns. TODO: Solve via SVD.";
203  return results;
204  }
205  x_updated = false;
206  }
207 
208  // Solve for dogleg direction dx_dl.
209  Scalar beta = 0;
210  Step step =
211  SolveDoglegDirection(dx_sd, dx_gn, radius, alpha, &dx_dl, &beta);
212 
213  Scalar e3 = params.relative_step_threshold;
214  if (dx_dl.norm() < e3 * (x.norm() + e3)) {
216  break;
217  }
218 
219  x_new = x + dx_dl;
220  Scalar actual = f_(x).squaredNorm() - f_(x_new).squaredNorm();
221  Scalar predicted = 0;
222  if (step == GAUSS_NEWTON) {
223  predicted = f_(x).squaredNorm();
224  } else if (step == STEEPEST_DESCENT) {
225  predicted = radius * (2 * alpha * g.norm() - radius) / 2 / alpha;
226  } else if (step == DOGLEG) {
227  predicted = 0.5 * alpha * (1 - beta) * (1 - beta) * g.squaredNorm() +
228  beta * (2 - beta) * f_(x).squaredNorm();
229  }
230  Scalar rho = actual / predicted;
231 
232  if (step == GAUSS_NEWTON)
233  printf(" GAUSS");
234  if (step == STEEPEST_DESCENT)
235  printf(" STEE");
236  if (step == DOGLEG)
237  printf(" DOGL");
238 
239  printf(" %12g %12g %12g\n", rho, actual, predicted);
240 
241  if (rho > 0) {
242  // Accept update because the linear model is a good fit.
243  x = x_new;
244  results.status = Update(x, params, &J, &A, &error, &g);
245  x_updated = true;
246  }
247  if (rho > 0.75) {
248  radius = std::max(radius, 3 * dx_dl.norm());
249  } else if (rho < 0.25) {
250  radius /= 2;
251  if (radius < e3 * (x.norm() + e3)) {
252  results.status = TRUST_REGION_TOO_SMALL;
253  }
254  }
255  }
256  if (results.status == RUNNING) {
257  results.status = HIT_MAX_ITERATIONS;
258  }
259  results.error_magnitude = error.norm();
260  results.gradient_magnitude = g.norm();
261  results.iterations = i;
262  return results;
263  }
264 
265  private:
266  const Function& f_;
267  Jacobian df_;
268 };
269 
270 } // namespace libmv
271 
272 #endif // LIBMV_NUMERIC_DOGLEG_H
sqrt(x)+1/max(0
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
#define A
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
Matrix< typename Function::FMatrixType::RealScalar, Function::FMatrixType::RowsAtCompileTime, Function::XMatrixType::RowsAtCompileTime > JMatrixType
Definition: dogleg.h:55
Results minimize(const SolverParameters &params, Parameters *x_and_min)
Definition: dogleg.h:158
Status Update(const Parameters &x, const SolverParameters &params, JMatrixType *J, AMatrixType *A, FVec *error, Parameters *g)
Definition: dogleg.h:99
Function::XMatrixType::RealScalar Scalar
Definition: dogleg.h:49
Function::FMatrixType FVec
Definition: dogleg.h:50
@ STEEPEST_DESCENT
Definition: dogleg.h:73
@ GAUSS_NEWTON
Definition: dogleg.h:72
@ ERROR_TOO_SMALL
Definition: dogleg.h:66
@ HIT_MAX_ITERATIONS
Definition: dogleg.h:67
@ TRUST_REGION_TOO_SMALL
Definition: dogleg.h:65
@ RELATIVE_STEP_SIZE_TOO_SMALL
Definition: dogleg.h:64
@ GRADIENT_TOO_SMALL
Definition: dogleg.h:63
Matrix< typename JMatrixType::RealScalar, JMatrixType::ColsAtCompileTime, JMatrixType::ColsAtCompileTime > AMatrixType
Definition: dogleg.h:59
Function::XMatrixType Parameters
Definition: dogleg.h:51
Dogleg(const Function &f)
Definition: dogleg.h:76
Results minimize(Parameters *x_and_min)
Definition: dogleg.h:153
Step SolveDoglegDirection(const Parameters &dx_sd, const Parameters &dx_gn, Scalar radius, Scalar alpha, Parameters *dx_dl, Scalar *beta)
Definition: dogleg.h:119
uiWidgetBaseParameters params[MAX_WIDGET_BASE_BATCH]
#define LOG(severity)
Definition: log.h:36
static void error(const char *str)
Definition: meshlaplacian.c:51
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
static const pxr::TfToken g("g", pxr::TfToken::Immortal)
Scalar gradient_magnitude
Definition: dogleg.h:94
Scalar error_magnitude
Definition: dogleg.h:93
float max
ccl_device_inline float beta(float x, float y)
Definition: util/math.h:775