blob: b6484fe2910e6836a30cd454fbde3125f7dd34c8 [file] [log] [blame]
Austin Schuh70cc9552019-01-21 19:46:48 -08001// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2017 Google Inc. All rights reserved.
3// http://ceres-solver.org/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9// this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11// this list of conditions and the following disclaimer in the documentation
12// and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14// used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: mierle@gmail.com (Keir Mierle)
30//
31// WARNING WARNING WARNING
32// WARNING WARNING WARNING Tiny solver is experimental and will change.
33// WARNING WARNING WARNING
34//
35// A tiny least squares solver using Levenberg-Marquardt, intended for solving
36// small dense problems with low latency and low overhead. The implementation
37// takes care to do all allocation up front, so that no memory is allocated
38// during solving. This is especially useful when solving many similar problems;
39// for example, inverse pixel distortion for every pixel on a grid.
40//
41// Note: This code has no dependencies beyond Eigen, including on other parts of
42// Ceres, so it is possible to take this file alone and put it in another
43// project without the rest of Ceres.
44//
45// Algorithm based off of:
46//
47// [1] K. Madsen, H. Nielsen, O. Tingleoff.
48// Methods for Non-linear Least Squares Problems.
49// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
50
51#ifndef CERES_PUBLIC_TINY_SOLVER_H_
52#define CERES_PUBLIC_TINY_SOLVER_H_
53
54#include <cassert>
55#include <cmath>
56
57#include "Eigen/Dense"
58
59namespace ceres {
60
61// To use tiny solver, create a class or struct that allows computing the cost
62// function (described below). This is similar to a ceres::CostFunction, but is
63// different to enable statically allocating all memory for the solver
64// (specifically, enum sizes). Key parts are the Scalar typedef, the enums to
65// describe problem sizes (needed to remove all heap allocations), and the
66// operator() overload to evaluate the cost and (optionally) jacobians.
67//
68// struct TinySolverCostFunctionTraits {
69// typedef double Scalar;
70// enum {
71// NUM_RESIDUALS = <int> OR Eigen::Dynamic,
72// NUM_PARAMETERS = <int> OR Eigen::Dynamic,
73// };
74// bool operator()(const double* parameters,
75// double* residuals,
76// double* jacobian) const;
77//
78// int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic.
79// int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic.
80// };
81//
82// For operator(), the size of the objects is:
83//
84// double* parameters -- NUM_PARAMETERS or NumParameters()
85// double* residuals -- NUM_RESIDUALS or NumResiduals()
86// double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format
87// (Eigen's default); or NULL if no jacobian requested.
88//
89// An example (fully statically sized):
90//
91// struct MyCostFunctionExample {
92// typedef double Scalar;
93// enum {
94// NUM_RESIDUALS = 2,
95// NUM_PARAMETERS = 3,
96// };
97// bool operator()(const double* parameters,
98// double* residuals,
99// double* jacobian) const {
100// residuals[0] = x + 2*y + 4*z;
101// residuals[1] = y * z;
102// if (jacobian) {
103// jacobian[0 * 2 + 0] = 1; // First column (x).
104// jacobian[0 * 2 + 1] = 0;
105//
106// jacobian[1 * 2 + 0] = 2; // Second column (y).
107// jacobian[1 * 2 + 1] = z;
108//
109// jacobian[2 * 2 + 0] = 4; // Third column (z).
110// jacobian[2 * 2 + 1] = y;
111// }
112// return true;
113// }
114// };
115//
116// The solver supports either statically or dynamically sized cost
117// functions. If the number of residuals is dynamic then the Function
118// must define:
119//
120// int NumResiduals() const;
121//
122// If the number of parameters is dynamic then the Function must
123// define:
124//
125// int NumParameters() const;
126//
127template<typename Function,
128 typename LinearSolver = Eigen::LDLT<
129 Eigen::Matrix<typename Function::Scalar,
130 Function::NUM_PARAMETERS,
131 Function::NUM_PARAMETERS>>>
132class TinySolver {
133 public:
134 enum {
135 NUM_RESIDUALS = Function::NUM_RESIDUALS,
136 NUM_PARAMETERS = Function::NUM_PARAMETERS
137 };
138 typedef typename Function::Scalar Scalar;
139 typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters;
140
141 enum Status {
142 GRADIENT_TOO_SMALL, // eps > max(J'*f(x))
143 RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / (||x|| + eps)
144 COST_TOO_SMALL, // eps > ||f(x)||^2 / 2
145 HIT_MAX_ITERATIONS,
146
147 // TODO(sameeragarwal): Deal with numerical failures.
148 };
149
150 struct Options {
151 Scalar gradient_tolerance = 1e-10; // eps > max(J'*f(x))
152 Scalar parameter_tolerance = 1e-8; // eps > ||dx|| / ||x||
153 Scalar cost_threshold = // eps > ||f(x)||
154 std::numeric_limits<Scalar>::epsilon();
155 Scalar initial_trust_region_radius = 1e4;
156 int max_num_iterations = 50;
157 };
158
159 struct Summary {
160 Scalar initial_cost = -1; // 1/2 ||f(x)||^2
161 Scalar final_cost = -1; // 1/2 ||f(x)||^2
162 Scalar gradient_max_norm = -1; // max(J'f(x))
163 int iterations = -1;
164 Status status = HIT_MAX_ITERATIONS;
165 };
166
167 bool Update(const Function& function, const Parameters &x) {
168 if (!function(x.data(), error_.data(), jacobian_.data())) {
169 return false;
170 }
171
172 error_ = -error_;
173
174 // On the first iteration, compute a diagonal (Jacobi) scaling
175 // matrix, which we store as a vector.
176 if (summary.iterations == 0) {
177 // jacobi_scaling = 1 / (1 + diagonal(J'J))
178 //
179 // 1 is added to the denominator to regularize small diagonal
180 // entries.
181 jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array());
182 }
183
184 // This explicitly computes the normal equations, which is numerically
185 // unstable. Nevertheless, it is often good enough and is fast.
186 //
187 // TODO(sameeragarwal): Refactor this to allow for DenseQR
188 // factorization.
189 jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal();
190 jtj_ = jacobian_.transpose() * jacobian_;
191 g_ = jacobian_.transpose() * error_;
192 summary.gradient_max_norm = g_.array().abs().maxCoeff();
193 cost_ = error_.squaredNorm() / 2;
194 return true;
195 }
196
197 const Summary& Solve(const Function& function, Parameters* x_and_min) {
198 Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function);
199 assert(x_and_min);
200 Parameters& x = *x_and_min;
201 summary = Summary();
202 summary.iterations = 0;
203
204 // TODO(sameeragarwal): Deal with failure here.
205 Update(function, x);
206 summary.initial_cost = cost_;
207 summary.final_cost = cost_;
208
209 if (summary.gradient_max_norm < options.gradient_tolerance) {
210 summary.status = GRADIENT_TOO_SMALL;
211 return summary;
212 }
213
214 if (cost_ < options.cost_threshold) {
215 summary.status = COST_TOO_SMALL;
216 return summary;
217 }
218
219 Scalar u = 1.0 / options.initial_trust_region_radius;
220 Scalar v = 2;
221
222 for (summary.iterations = 1;
223 summary.iterations < options.max_num_iterations;
224 summary.iterations++) {
225 jtj_regularized_ = jtj_;
226 const Scalar min_diagonal = 1e-6;
227 const Scalar max_diagonal = 1e32;
228 for (int i = 0; i < lm_diagonal_.rows(); ++i) {
229 lm_diagonal_[i] = std::sqrt(
230 u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal));
231 jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i];
232 }
233
234 // TODO(sameeragarwal): Check for failure and deal with it.
235 linear_solver_.compute(jtj_regularized_);
236 lm_step_ = linear_solver_.solve(g_);
237 dx_ = jacobi_scaling_.asDiagonal() * lm_step_;
238
239 // Adding parameter_tolerance to x.norm() ensures that this
240 // works if x is near zero.
241 const Scalar parameter_tolerance =
242 options.parameter_tolerance *
243 (x.norm() + options.parameter_tolerance);
244 if (dx_.norm() < parameter_tolerance) {
245 summary.status = RELATIVE_STEP_SIZE_TOO_SMALL;
246 break;
247 }
248 x_new_ = x + dx_;
249
250 // TODO(keir): Add proper handling of errors from user eval of cost
251 // functions.
252 function(&x_new_[0], &f_x_new_[0], NULL);
253
254 const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm());
255
256 // TODO(sameeragarwal): Better more numerically stable evaluation.
257 const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_);
258
259 // rho is the ratio of the actual reduction in error to the reduction
260 // in error that would be obtained if the problem was linear. See [1]
261 // for details.
262 Scalar rho(cost_change / model_cost_change);
263 if (rho > 0) {
264 // Accept the Levenberg-Marquardt step because the linear
265 // model fits well.
266 x = x_new_;
267
268 // TODO(sameeragarwal): Deal with failure.
269 Update(function, x);
270 if (summary.gradient_max_norm < options.gradient_tolerance) {
271 summary.status = GRADIENT_TOO_SMALL;
272 break;
273 }
274
275 if (cost_ < options.cost_threshold) {
276 summary.status = COST_TOO_SMALL;
277 break;
278 }
279
280 Scalar tmp = Scalar(2 * rho - 1);
281 u = u * std::max(1 / 3., 1 - tmp * tmp * tmp);
282 v = 2;
283 continue;
284 }
285
286 // Reject the update because either the normal equations failed to solve
287 // or the local linear model was not good (rho < 0). Instead, increase u
288 // to move closer to gradient descent.
289 u *= v;
290 v *= 2;
291 }
292
293 summary.final_cost = cost_;
294 return summary;
295 }
296
297 Options options;
298 Summary summary;
299
300 private:
301 // Preallocate everything, including temporary storage needed for solving the
302 // linear system. This allows reusing the intermediate storage across solves.
303 LinearSolver linear_solver_;
304 Scalar cost_;
305 Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_;
306 Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_;
307 Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;
308 Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;
309
310 // The following definitions are needed for template metaprogramming.
311 template <bool Condition, typename T>
312 struct enable_if;
313
314 template <typename T>
315 struct enable_if<true, T> {
316 typedef T type;
317 };
318
319 // The number of parameters and residuals are dynamically sized.
320 template <int R, int P>
321 typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type
322 Initialize(const Function& function) {
323 Initialize(function.NumResiduals(), function.NumParameters());
324 }
325
326 // The number of parameters is dynamically sized and the number of
327 // residuals is statically sized.
328 template <int R, int P>
329 typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type
330 Initialize(const Function& function) {
331 Initialize(function.NumResiduals(), P);
332 }
333
334 // The number of parameters is statically sized and the number of
335 // residuals is dynamically sized.
336 template <int R, int P>
337 typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type
338 Initialize(const Function& function) {
339 Initialize(R, function.NumParameters());
340 }
341
342 // The number of parameters and residuals are statically sized.
343 template <int R, int P>
344 typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type
345 Initialize(const Function& /* function */) {}
346
347 void Initialize(int num_residuals, int num_parameters) {
348 dx_.resize(num_parameters);
349 x_new_.resize(num_parameters);
350 g_.resize(num_parameters);
351 jacobi_scaling_.resize(num_parameters);
352 lm_diagonal_.resize(num_parameters);
353 lm_step_.resize(num_parameters);
354 error_.resize(num_residuals);
355 f_x_new_.resize(num_residuals);
356 jacobian_.resize(num_residuals, num_parameters);
357 jtj_.resize(num_parameters, num_parameters);
358 jtj_regularized_.resize(num_parameters, num_parameters);
359 }
360};
361
362} // namespace ceres
363
364#endif // CERES_PUBLIC_TINY_SOLVER_H_