1
// Ceres Solver - A fast non-linear least squares minimizer
2
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3
// http://code.google.com/p/ceres-solver/
5
// Redistribution and use in source and binary forms, with or without
6
// modification, are permitted provided that the following conditions are met:
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.
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.
29
// Author: keir@google.com (Keir Mierle)
31
// Create CostFunctions as needed by the least squares framework with jacobians
32
// computed via numeric (a.k.a. finite) differentiation. For more details see
33
// http://en.wikipedia.org/wiki/Numerical_differentiation.
35
// To get a numerically differentiated cost function, define a subclass of
36
// CostFunction such that the Evaluate() function ignores the jacobian
37
// parameter. The numeric differentiation wrapper will fill in the jacobian
38
// parameter if nececssary by repeatedly calling the Evaluate() function with
39
// small changes to the appropriate parameters, and computing the slope. For
40
// performance, the numeric differentiation wrapper class is templated on the
41
// concrete cost function, even though it could be implemented only in terms of
42
// the virtual CostFunction interface.
44
// The numerically differentiated version of a cost function for a cost function
45
// can be constructed as follows:
47
// CostFunction* cost_function
48
// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
49
// new MyCostFunction(...), TAKE_OWNERSHIP);
51
// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
52
// respectively. Look at the tests for a more detailed example.
54
// The central difference method is considerably more accurate at the cost of
55
// twice as many function evaluations than forward difference. Consider using
56
// central differences begin with, and only after that works, trying forward
57
// difference to improve performance.
59
// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
61
#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
62
#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
65
#include <glog/logging.h>
66
#include "Eigen/Dense"
67
#include "ceres/internal/scoped_ptr.h"
68
#include "ceres/sized_cost_function.h"
69
#include "ceres/types.h"
73
enum NumericDiffMethod {
78
// This is split from the main class because C++ doesn't allow partial template
79
// specializations for member functions. The alternative is to repeat the main
80
// class for differing numbers of parameters, which is also unfortunate.
81
template <typename CostFunctionNoJacobian,
83
int parameter_block_size,
85
NumericDiffMethod method>
87
// Mutates parameters but must restore them before return.
88
static bool EvaluateJacobianForParameterBlock(
89
const CostFunctionNoJacobian *function,
90
double const* residuals_at_eval_point,
95
using Eigen::RowMajor;
97
typedef Matrix<double, num_residuals, 1> ResidualVector;
98
typedef Matrix<double, parameter_block_size, 1> ParameterVector;
99
typedef Matrix<double, num_residuals, parameter_block_size, RowMajor>
102
Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block],
104
parameter_block_size);
106
// Mutate 1 element at a time and then restore.
107
Map<ParameterVector> x_plus_delta(parameters[parameter_block],
108
parameter_block_size);
109
ParameterVector x(x_plus_delta);
111
// TODO(keir): Pick a smarter number! In theory a good choice is sqrt(eps) *
112
// x, which for doubles means about 1e-8 * x. However, I have found this
113
// number too optimistic. This number should be exposed for users to change.
114
const double kRelativeStepSize = 1e-6;
116
ParameterVector step_size = x.array().abs() * kRelativeStepSize;
118
// To handle cases where a parameter is exactly zero, instead use the mean
119
// step_size for the other dimensions.
120
double fallback_step_size = step_size.sum() / step_size.rows();
121
if (fallback_step_size == 0.0) {
122
// If all the parameters are zero, there's no good answer. Take
123
// kRelativeStepSize as a guess and hope for the best.
124
fallback_step_size = kRelativeStepSize;
127
// For each parameter in the parameter block, use finite differences to
128
// compute the derivative for that parameter.
129
for (int j = 0; j < parameter_block_size; ++j) {
130
if (step_size(j) == 0.0) {
131
// The parameter is exactly zero, so compromise and use the mean
132
// step_size from the other parameters. This can break in many cases,
133
// but it's hard to pick a good number without problem specific
135
step_size(j) = fallback_step_size;
137
x_plus_delta(j) = x(j) + step_size(j);
139
double residuals[num_residuals]; // NOLINT
140
if (!function->Evaluate(parameters, residuals, NULL)) {
141
// Something went wrong; bail.
145
// Compute this column of the jacobian in 3 steps:
146
// 1. Store residuals for the forward part.
147
// 2. Subtract residuals for the backward (or 0) part.
148
// 3. Divide out the run.
149
parameter_jacobian.col(j) =
150
Map<const ResidualVector>(residuals, num_residuals);
152
double one_over_h = 1 / step_size(j);
153
if (method == CENTRAL) {
154
// Compute the function on the other side of x(j).
155
x_plus_delta(j) = x(j) - step_size(j);
157
if (!function->Evaluate(parameters, residuals, NULL)) {
158
// Something went wrong; bail.
161
parameter_jacobian.col(j) -=
162
Map<ResidualVector>(residuals, num_residuals, 1);
165
// Forward difference only; reuse existing residuals evaluation.
166
parameter_jacobian.col(j) -=
167
Map<const ResidualVector>(residuals_at_eval_point, num_residuals);
169
x_plus_delta(j) = x(j); // Restore x_plus_delta.
171
// Divide out the run to get slope.
172
parameter_jacobian.col(j) *= one_over_h;
178
// Prevent invalid instantiations.
179
template <typename CostFunctionNoJacobian,
182
NumericDiffMethod method>
183
struct Differencer<CostFunctionNoJacobian,
185
0 /* parameter_block_size */,
188
static bool EvaluateJacobianForParameterBlock(
189
const CostFunctionNoJacobian *function,
190
double const* residuals_at_eval_point,
192
double **jacobians) {
193
LOG(FATAL) << "Shouldn't get here.";
198
template <typename CostFunctionNoJacobian,
199
NumericDiffMethod method = CENTRAL, int M = 0,
200
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0>
201
class NumericDiffCostFunction
202
: public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> {
204
NumericDiffCostFunction(CostFunctionNoJacobian* function,
206
: function_(function), ownership_(ownership) {}
208
virtual ~NumericDiffCostFunction() {
209
if (ownership_ != TAKE_OWNERSHIP) {
214
virtual bool Evaluate(double const* const* parameters,
216
double** jacobians) const {
217
// Get the function value (residuals) at the the point to evaluate.
218
bool success = function_->Evaluate(parameters, residuals, NULL);
220
// Something went wrong; ignore the jacobian.
224
// Nothing to do; just forward.
228
// Create a copy of the parameters which will get mutated.
229
const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5;
230
double parameters_copy[kParametersSize];
231
double *parameters_references_copy[6];
232
parameters_references_copy[0] = ¶meters_copy[0];
233
parameters_references_copy[1] = ¶meters_copy[0] + N0;
234
parameters_references_copy[2] = ¶meters_copy[0] + N0 + N1;
235
parameters_references_copy[3] = ¶meters_copy[0] + N0 + N1 + N2;
236
parameters_references_copy[4] = ¶meters_copy[0] + N0 + N1 + N2 + N3;
237
parameters_references_copy[5] =
238
¶meters_copy[0] + N0 + N1 + N2 + N3 + N4;
240
#define COPY_PARAMETER_BLOCK(block) \
241
if (N ## block) memcpy(parameters_references_copy[block], \
243
sizeof(double) * N ## block); // NOLINT
244
COPY_PARAMETER_BLOCK(0);
245
COPY_PARAMETER_BLOCK(1);
246
COPY_PARAMETER_BLOCK(2);
247
COPY_PARAMETER_BLOCK(3);
248
COPY_PARAMETER_BLOCK(4);
249
COPY_PARAMETER_BLOCK(5);
250
#undef COPY_PARAMETER_BLOCK
252
#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \
253
if (N ## block && jacobians[block]) { \
254
if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \
258
method>::EvaluateJacobianForParameterBlock( \
261
parameters_references_copy, \
266
EVALUATE_JACOBIAN_FOR_BLOCK(0);
267
EVALUATE_JACOBIAN_FOR_BLOCK(1);
268
EVALUATE_JACOBIAN_FOR_BLOCK(2);
269
EVALUATE_JACOBIAN_FOR_BLOCK(3);
270
EVALUATE_JACOBIAN_FOR_BLOCK(4);
271
EVALUATE_JACOBIAN_FOR_BLOCK(5);
272
#undef EVALUATE_JACOBIAN_FOR_BLOCK
277
internal::scoped_ptr<CostFunctionNoJacobian> function_;
278
Ownership ownership_;
283
#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
added
removed
include/ceres/numeric_diff_cost_function.h
