1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
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)
30 // sameeragarwal@google.com (Sameer Agarwal)
32 // Create CostFunctions as needed by the least squares framework with jacobians
33 // computed via numeric (a.k.a. finite) differentiation. For more details see
34 // http://en.wikipedia.org/wiki/Numerical_differentiation.
36 // To get an numerically differentiated cost function, you must define
37 // a class with a operator() (a functor) that computes the residuals.
39 // The function must write the computed value in the last argument
40 // (the only non-const one) and return true to indicate success.
41 // Please see cost_function.h for details on how the return value
42 // maybe used to impose simple constraints on the parameter block.
44 // For example, consider a scalar error e = k - x'y, where both x and y are
45 // two-dimensional column vector parameters, the prime sign indicates
46 // transposition, and k is a constant. The form of this error, which is the
47 // difference between a constant and an expression, is a common pattern in least
48 // squares problems. For example, the value x'y might be the model expectation
49 // for a series of measurements, where there is an instance of the cost function
50 // for each measurement k.
52 // The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
53 // the squaring is implicitly done by the optimization framework.
55 // To write an numerically-differentiable cost function for the above model, first
58 // class MyScalarCostFunctor {
59 // MyScalarCostFunctor(double k): k_(k) {}
61 // bool operator()(const double* const x,
62 // const double* const y,
63 // double* residuals) const {
64 // residuals[0] = k_ - x[0] * y[0] + x[1] * y[1];
72 // Note that in the declaration of operator() the input parameters x
73 // and y come first, and are passed as const pointers to arrays of
74 // doubles. If there were three input parameters, then the third input
75 // parameter would come after y. The output is always the last
76 // parameter, and is also a pointer to an array. In the example above,
77 // the residual is a scalar, so only residuals[0] is set.
79 // Then given this class definition, the numerically differentiated
80 // cost function with central differences used for computing the
81 // derivative can be constructed as follows.
83 // CostFunction* cost_function
84 // = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
85 // new MyScalarCostFunctor(1.0)); ^ ^ ^ ^
87 // Finite Differencing Scheme -+ | | |
88 // Dimension of residual ------------+ | |
89 // Dimension of x ----------------------+ |
90 // Dimension of y -------------------------+
92 // In this example, there is usually an instance for each measurement of k.
94 // In the instantiation above, the template parameters following
95 // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing
96 // a 1-dimensional output from two arguments, both 2-dimensional.
98 // NumericDiffCostFunction also supports cost functions with a
99 // runtime-determined number of residuals. For example:
101 // CostFunction* cost_function
102 // = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
103 // new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^
104 // TAKE_OWNERSHIP, | | |
105 // runtime_number_of_residuals); <----+ | | |
108 // Actual number of residuals ------+ | | |
109 // Indicate dynamic number of residuals --------------------+ | |
110 // Dimension of x ------------------------------------------------+ |
111 // Dimension of y ---------------------------------------------------+
113 // The framework can currently accommodate cost functions of up to 10
114 // independent variables, and there is no limit on the dimensionality
117 // The central difference method is considerably more accurate at the cost of
118 // twice as many function evaluations than forward difference. Consider using
119 // central differences begin with, and only after that works, trying forward
120 // difference to improve performance.
122 // WARNING #1: A common beginner's error when first using
123 // NumericDiffCostFunction is to get the sizing wrong. In particular,
124 // there is a tendency to set the template parameters to (dimension of
125 // residual, number of parameters) instead of passing a dimension
126 // parameter for *every parameter*. In the example above, that would
127 // be <MyScalarCostFunctor, 1, 2>, which is missing the last '2'
128 // argument. Please be careful when setting the size parameters.
130 ////////////////////////////////////////////////////////////////////////////
131 ////////////////////////////////////////////////////////////////////////////
133 // ALTERNATE INTERFACE
135 // For a variety of reasons, including compatibility with legacy code,
136 // NumericDiffCostFunction can also take CostFunction objects as
137 // input. The following describes how.
139 // To get a numerically differentiated cost function, define a
140 // subclass of CostFunction such that the Evaluate() function ignores
141 // the jacobian parameter. The numeric differentiation wrapper will
142 // fill in the jacobian parameter if necessary by repeatedly calling
143 // the Evaluate() function with small changes to the appropriate
144 // parameters, and computing the slope. For performance, the numeric
145 // differentiation wrapper class is templated on the concrete cost
146 // function, even though it could be implemented only in terms of the
147 // virtual CostFunction interface.
149 // The numerically differentiated version of a cost function for a cost function
150 // can be constructed as follows:
152 // CostFunction* cost_function
153 // = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
154 // new MyCostFunction(...), TAKE_OWNERSHIP);
156 // where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
157 // respectively. Look at the tests for a more detailed example.
159 // TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
161 #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
162 #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
164 #include "Eigen/Dense"
165 #include "ceres/cost_function.h"
166 #include "ceres/internal/numeric_diff.h"
167 #include "ceres/internal/scoped_ptr.h"
168 #include "ceres/numeric_diff_options.h"
169 #include "ceres/sized_cost_function.h"
170 #include "ceres/types.h"
171 #include "glog/logging.h"
175 template <typename CostFunctor,
176 NumericDiffMethodType method = CENTRAL,
177 int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC
178 int N0 = 0, // Number of parameters in block 0.
179 int N1 = 0, // Number of parameters in block 1.
180 int N2 = 0, // Number of parameters in block 2.
181 int N3 = 0, // Number of parameters in block 3.
182 int N4 = 0, // Number of parameters in block 4.
183 int N5 = 0, // Number of parameters in block 5.
184 int N6 = 0, // Number of parameters in block 6.
185 int N7 = 0, // Number of parameters in block 7.
186 int N8 = 0, // Number of parameters in block 8.
187 int N9 = 0> // Number of parameters in block 9.
188 class NumericDiffCostFunction
189 : public SizedCostFunction<kNumResiduals,
191 N5, N6, N7, N8, N9> {
193 NumericDiffCostFunction(
194 CostFunctor* functor,
195 Ownership ownership = TAKE_OWNERSHIP,
196 int num_residuals = kNumResiduals,
197 const NumericDiffOptions& options = NumericDiffOptions())
199 ownership_(ownership),
201 if (kNumResiduals == DYNAMIC) {
202 SizedCostFunction<kNumResiduals,
205 ::set_num_residuals(num_residuals);
209 ~NumericDiffCostFunction() {
210 if (ownership_ != TAKE_OWNERSHIP) {
215 virtual bool Evaluate(double const* const* parameters,
217 double** jacobians) const {
218 using internal::FixedArray;
219 using internal::NumericDiff;
221 const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
222 const int kNumParameterBlocks =
223 (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
224 (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
226 // Get the function value (residuals) at the the point to evaluate.
227 if (!internal::EvaluateImpl<CostFunctor,
228 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
236 if (jacobians == NULL) {
240 // Create a copy of the parameters which will get mutated.
241 FixedArray<double> parameters_copy(kNumParameters);
242 FixedArray<double*> parameters_reference_copy(kNumParameterBlocks);
244 parameters_reference_copy[0] = parameters_copy.get();
245 if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0;
246 if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1;
247 if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2;
248 if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3;
249 if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4;
250 if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5;
251 if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6;
252 if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7;
253 if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8;
255 #define CERES_COPY_PARAMETER_BLOCK(block) \
256 if (N ## block) memcpy(parameters_reference_copy[block], \
258 sizeof(double) * N ## block); // NOLINT
260 CERES_COPY_PARAMETER_BLOCK(0);
261 CERES_COPY_PARAMETER_BLOCK(1);
262 CERES_COPY_PARAMETER_BLOCK(2);
263 CERES_COPY_PARAMETER_BLOCK(3);
264 CERES_COPY_PARAMETER_BLOCK(4);
265 CERES_COPY_PARAMETER_BLOCK(5);
266 CERES_COPY_PARAMETER_BLOCK(6);
267 CERES_COPY_PARAMETER_BLOCK(7);
268 CERES_COPY_PARAMETER_BLOCK(8);
269 CERES_COPY_PARAMETER_BLOCK(9);
271 #undef CERES_COPY_PARAMETER_BLOCK
273 #define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block) \
274 if (N ## block && jacobians[block] != NULL) { \
275 if (!NumericDiff<CostFunctor, \
278 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, \
280 N ## block >::EvaluateJacobianForParameterBlock( \
284 SizedCostFunction<kNumResiduals, \
285 N0, N1, N2, N3, N4, \
286 N5, N6, N7, N8, N9>::num_residuals(), \
289 parameters_reference_copy.get(), \
290 jacobians[block])) { \
295 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0);
296 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1);
297 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2);
298 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3);
299 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4);
300 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5);
301 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6);
302 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7);
303 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8);
304 CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9);
306 #undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK
312 internal::scoped_ptr<CostFunctor> functor_;
313 Ownership ownership_;
314 NumericDiffOptions options_;
319 #endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_