Blender  V3.3
levenberg_marquardt.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 levenberg marquardt.
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_LEVENBERG_MARQUARDT_H
30 #define LIBMV_NUMERIC_LEVENBERG_MARQUARDT_H
31 
32 #include <cmath>
33 
34 #include "libmv/logging/logging.h"
36 #include "libmv/numeric/numeric.h"
37 
38 namespace libmv {
39 
40 template <typename Function,
41  typename Jacobian = NumericJacobian<Function>,
42  typename Solver = Eigen::PartialPivLU<
43  Matrix<typename Function::FMatrixType::RealScalar,
44  Function::XMatrixType::RowsAtCompileTime,
45  Function::XMatrixType::RowsAtCompileTime>>>
47  public:
48  typedef typename Function::XMatrixType::RealScalar Scalar;
49  typedef typename Function::FMatrixType FVec;
50  typedef typename Function::XMatrixType Parameters;
51  typedef Matrix<typename Function::FMatrixType::RealScalar,
52  Function::FMatrixType::RowsAtCompileTime,
53  Function::XMatrixType::RowsAtCompileTime>
55  typedef Matrix<typename JMatrixType::RealScalar,
56  JMatrixType::ColsAtCompileTime,
57  JMatrixType::ColsAtCompileTime>
59 
60  // TODO(keir): Some of these knobs can be derived from each other and
61  // removed, instead of requiring the user to set them.
62  enum Status {
64  GRADIENT_TOO_SMALL, // eps > max(J'*f(x))
65  RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / ||x||
66  ERROR_TOO_SMALL, // eps > ||f(x)||
68  };
69 
70  LevenbergMarquardt(const Function& f) : f_(f), df_(f) {}
71 
74  : gradient_threshold(1e-16),
76  error_threshold(1e-16),
78  max_iterations(100) {}
79  Scalar gradient_threshold; // eps > max(J'*f(x))
80  Scalar relative_step_threshold; // eps > ||dx|| / ||x||
81  Scalar error_threshold; // eps > ||f(x)||
82  Scalar initial_scale_factor; // Initial u for solving normal equations.
83  int max_iterations; // Maximum number of solver iterations.
84  };
85 
86  struct Results {
87  Scalar error_magnitude; // ||f(x)||
88  Scalar gradient_magnitude; // ||J'f(x)||
91  };
92 
94  const SolverParameters& params,
95  JMatrixType* J,
96  AMatrixType* A,
97  FVec* error,
98  Parameters* g) {
99  *J = df_(x);
100  *A = (*J).transpose() * (*J);
101  *error = -f_(x);
102  *g = (*J).transpose() * *error;
103  if (g->array().abs().maxCoeff() < params.gradient_threshold) {
104  return GRADIENT_TOO_SMALL;
105  } else if (error->norm() < params.error_threshold) {
106  return ERROR_TOO_SMALL;
107  }
108  return RUNNING;
109  }
110 
113  minimize(params, x_and_min);
114  }
115 
117  Parameters& x = *x_and_min;
118  JMatrixType J;
119  AMatrixType A;
120  FVec error;
121  Parameters g;
122 
123  Results results;
124  results.status = Update(x, params, &J, &A, &error, &g);
125 
126  Scalar u = Scalar(params.initial_scale_factor * A.diagonal().maxCoeff());
127  Scalar v = 2;
128 
129  Parameters dx, x_new;
130  int i;
131  for (i = 0; results.status == RUNNING && i < params.max_iterations; ++i) {
132  VLOG(3) << "iteration: " << i;
133  VLOG(3) << "||f(x)||: " << f_(x).norm();
134  VLOG(3) << "max(g): " << g.array().abs().maxCoeff();
135  VLOG(3) << "u: " << u;
136  VLOG(3) << "v: " << v;
137 
138  AMatrixType A_augmented =
139  A + u * AMatrixType::Identity(J.cols(), J.cols());
140  Solver solver(A_augmented);
141  dx = solver.solve(g);
142  bool solved = (A_augmented * dx).isApprox(g);
143  if (!solved) {
144  LOG(ERROR) << "Failed to solve";
145  }
146  if (solved && dx.norm() <= params.relative_step_threshold * x.norm()) {
148  break;
149  }
150  if (solved) {
151  x_new = x + dx;
152  // Rho is the ratio of the actual reduction in error to the reduction
153  // in error that would be obtained if the problem was linear.
154  // See [1] for details.
155  Scalar rho((error.squaredNorm() - f_(x_new).squaredNorm()) /
156  dx.dot(u * dx + g));
157  if (rho > 0) {
158  // Accept the Gauss-Newton step because the linear model fits well.
159  x = x_new;
160  results.status = Update(x, params, &J, &A, &error, &g);
161  Scalar tmp = Scalar(2 * rho - 1);
162  u = u * std::max(1 / 3., 1 - (tmp * tmp * tmp));
163  v = 2;
164  continue;
165  }
166  }
167  // Reject the update because either the normal equations failed to solve
168  // or the local linear model was not good (rho < 0). Instead, increase u
169  // to move closer to gradient descent.
170  u *= v;
171  v *= 2;
172  }
173  if (results.status == RUNNING) {
174  results.status = HIT_MAX_ITERATIONS;
175  }
176  results.error_magnitude = error.norm();
177  results.gradient_magnitude = g.norm();
178  results.iterations = i;
179  return results;
180  }
181 
182  private:
183  const Function& f_;
184  Jacobian df_;
185 };
186 
187 } // namespace libmv
188 
189 #endif // LIBMV_NUMERIC_LEVENBERG_MARQUARDT_H
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
ATTR_WARN_UNUSED_RESULT const BMVert * v
#define A
Status Update(const Parameters &x, const SolverParameters &params, JMatrixType *J, AMatrixType *A, FVec *error, Parameters *g)
Function::FMatrixType FVec
Matrix< typename JMatrixType::RealScalar, JMatrixType::ColsAtCompileTime, JMatrixType::ColsAtCompileTime > AMatrixType
Function::XMatrixType::RealScalar Scalar
Function::XMatrixType Parameters
LevenbergMarquardt(const Function &f)
Matrix< typename Function::FMatrixType::RealScalar, Function::FMatrixType::RowsAtCompileTime, Function::XMatrixType::RowsAtCompileTime > JMatrixType
Results minimize(const SolverParameters &params, Parameters *x_and_min)
Results minimize(Parameters *x_and_min)
uiWidgetBaseParameters params[MAX_WIDGET_BASE_BATCH]
#define VLOG(severity)
Definition: log.h:37
#define LOG(severity)
Definition: log.h:36
static void error(const char *str)
Definition: meshlaplacian.c:51
static const pxr::TfToken g("g", pxr::TfToken::Immortal)
float max