Blender V5.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
38
39namespace libmv {
40
41template <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>>>
47class 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
75
76 Dogleg(const Function& f) : f_(f), df_(f) {}
77
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
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 // Solve for Dogleg step dx_dl.
126 if (dx_gn.norm() < radius) {
127 *dx_dl = dx_gn;
128 return GAUSS_NEWTON;
129
130 } else if (alpha * dx_sd.norm() > radius) {
131 *dx_dl = (radius / dx_sd.norm()) * dx_sd;
132 return STEEPEST_DESCENT;
133
134 } else {
135 Parameters a = alpha * dx_sd;
136 const Parameters& b = dx_gn;
137 Parameters b_minus_a = a - b;
138 Scalar Mbma2 = b_minus_a.squaredNorm();
139 Scalar Ma2 = a.squaredNorm();
140 Scalar c = a.dot(b_minus_a);
141 Scalar radius2 = radius * radius;
142 if (c <= 0) {
143 *beta = (-c + sqrt(c * c + Mbma2 * (radius2 - Ma2))) / (Mbma2);
144 } else {
145 *beta = (radius2 - Ma2) / (c + sqrt(c * c + Mbma2 * (radius2 - Ma2)));
146 }
147 *dx_dl = alpha * dx_sd + (*beta) * (dx_gn - alpha * dx_sd);
148 return DOGLEG;
149 }
150 }
151
153 SolverParameters params;
154 return minimize(params, x_and_min);
155 }
156
157 Results minimize(const SolverParameters& params, Parameters* x_and_min) {
158 Parameters& x = *x_and_min;
159 JMatrixType J;
161 FVec error;
162 Parameters g;
163
164 Results results;
165 results.status = Update(x, params, &J, &A, &error, &g);
166
167 Scalar radius = params.initial_trust_radius;
168 bool x_updated = true;
169
170 Parameters x_new;
171 Parameters dx_sd; // Steepest descent step.
172 Parameters dx_dl; // Dogleg step.
173 Parameters dx_gn; // Gauss-Newton step.
174 printf("iteration ||f(x)|| max(g) radius\n");
175 int i = 0;
176 for (; results.status == RUNNING && i < params.max_iterations; ++i) {
177 printf("%9d %12g %12g %12g",
178 i,
179 f_(x).norm(),
180 g.array().abs().maxCoeff(),
181 radius);
182
183 // LG << "iteration: " << i;
184 // LG << "||f(x)||: " << f_(x).norm();
185 // LG << "max(g): " << g.cwise().abs().maxCoeff();
186 // LG << "radius: " << radius;
187 // Eqn 3.19 from [1]
188 Scalar alpha = g.squaredNorm() / (J * g).squaredNorm();
189
190 // Solve for steepest descent direction dx_sd.
191 dx_sd = -g;
192
193 // Solve for Gauss-Newton direction dx_gn.
194 if (x_updated) {
195 // TODO(keir): See Appendix B of [1] for discussion of when A is
196 // singular and there are many solutions. Solving that involves the SVD
197 // and is slower, but should still work.
198 Solver solver(A);
199 dx_gn = solver.solve(-g);
200 if (!(A * dx_gn).isApprox(-g)) {
201 LOG(ERROR) << "Failed to solve normal eqns. TODO: Solve via SVD.";
202 return results;
203 }
204 x_updated = false;
205 }
206
207 // Solve for dogleg direction dx_dl.
208 Scalar beta = 0;
209 Step step =
210 SolveDoglegDirection(dx_sd, dx_gn, radius, alpha, &dx_dl, &beta);
211
213 if (dx_dl.norm() < e3 * (x.norm() + e3)) {
215 break;
216 }
217
218 x_new = x + dx_dl;
219 Scalar actual = f_(x).squaredNorm() - f_(x_new).squaredNorm();
220 Scalar predicted = 0;
221 if (step == GAUSS_NEWTON) {
222 predicted = f_(x).squaredNorm();
223 } else if (step == STEEPEST_DESCENT) {
224 predicted = radius * (2 * alpha * g.norm() - radius) / 2 / alpha;
225 } else if (step == DOGLEG) {
226 predicted = 0.5 * alpha * (1 - beta) * (1 - beta) * g.squaredNorm() +
227 beta * (2 - beta) * f_(x).squaredNorm();
228 }
229 Scalar rho = actual / predicted;
230
231 if (step == GAUSS_NEWTON) {
232 printf(" GAUSS");
233 }
234 if (step == STEEPEST_DESCENT) {
235 printf(" STEE");
236 }
237 if (step == DOGLEG) {
238 printf(" DOGL");
239 }
240
241 printf(" %12g %12g %12g\n", rho, actual, predicted);
242
243 if (rho > 0) {
244 // Accept update because the linear model is a good fit.
245 x = x_new;
246 results.status = Update(x, params, &J, &A, &error, &g);
247 x_updated = true;
248 }
249 if (rho > 0.75) {
250 radius = std::max(radius, 3 * dx_dl.norm());
251 } else if (rho < 0.25) {
252 radius /= 2;
253 if (radius < e3 * (x.norm() + e3)) {
255 }
256 }
257 }
258 if (results.status == RUNNING) {
259 results.status = HIT_MAX_ITERATIONS;
260 }
261 results.error_magnitude = error.norm();
262 results.gradient_magnitude = g.norm();
263 results.iterations = i;
264 return results;
265 }
266
267 private:
268 const Function& f_;
269 Jacobian df_;
270};
271
272} // namespace libmv
273
274#endif // LIBMV_NUMERIC_DOGLEG_H
#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:157
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
@ 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:152
Step SolveDoglegDirection(const Parameters &dx_sd, const Parameters &dx_gn, Scalar radius, Scalar alpha, Parameters *dx_dl, Scalar *beta)
Definition dogleg.h:119
#define printf
#define sqrt
VecBase< float, D > step(VecOp< float, D >, VecOp< float, D >) RET
#define LOG(level)
Definition log.h:97
ccl_device_inline float beta(const float x, const float y)
Definition math_base.h:708
Scalar gradient_magnitude
Definition dogleg.h:94