fa96078df0274e6a59611884fb0759b0e8cbf79b
[blender.git] / extern / ceres / include / ceres / numeric_diff_cost_function.h
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 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: keir@google.com (Keir Mierle)
30 //         sameeragarwal@google.com (Sameer Agarwal)
31 //
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.
35 //
36 // To get an numerically differentiated cost function, you must define
37 // a class with a operator() (a functor) that computes the residuals.
38 //
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.
43 //
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.
51 //
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.
54 //
55 // To write an numerically-differentiable cost function for the above model, first
56 // define the object
57 //
58 //   class MyScalarCostFunctor {
59 //     MyScalarCostFunctor(double k): k_(k) {}
60 //
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];
65 //       return true;
66 //     }
67 //
68 //    private:
69 //     double k_;
70 //   };
71 //
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.
78 //
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.
82 //
83 //   CostFunction* cost_function
84 //       = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
85 //           new MyScalarCostFunctor(1.0));                    ^     ^  ^  ^
86 //                                                             |     |  |  |
87 //                                 Finite Differencing Scheme -+     |  |  |
88 //                                 Dimension of residual ------------+  |  |
89 //                                 Dimension of x ----------------------+  |
90 //                                 Dimension of y -------------------------+
91 //
92 // In this example, there is usually an instance for each measurement of k.
93 //
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.
97 //
98 // NumericDiffCostFunction also supports cost functions with a
99 // runtime-determined number of residuals. For example:
100 //
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); <----+                       |     |  |
106 //                                              |                       |     |  |
107 //                                              |                       |     |  |
108 //             Actual number of residuals ------+                       |     |  |
109 //             Indicate dynamic number of residuals --------------------+     |  |
110 //             Dimension of x ------------------------------------------------+  |
111 //             Dimension of y ---------------------------------------------------+
112 //
113 // The framework can currently accommodate cost functions of up to 10
114 // independent variables, and there is no limit on the dimensionality
115 // of each of them.
116 //
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.
121 //
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.
129 //
130 ////////////////////////////////////////////////////////////////////////////
131 ////////////////////////////////////////////////////////////////////////////
132 //
133 // ALTERNATE INTERFACE
134 //
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.
138 //
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.
148 //
149 // The numerically differentiated version of a cost function for a cost function
150 // can be constructed as follows:
151 //
152 //   CostFunction* cost_function
153 //       = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
154 //           new MyCostFunction(...), TAKE_OWNERSHIP);
155 //
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.
158 //
159 // TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
160
161 #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
162 #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
163
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"
172
173 namespace ceres {
174
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,
190                                N0, N1, N2, N3, N4,
191                                N5, N6, N7, N8, N9> {
192  public:
193   NumericDiffCostFunction(
194       CostFunctor* functor,
195       Ownership ownership = TAKE_OWNERSHIP,
196       int num_residuals = kNumResiduals,
197       const NumericDiffOptions& options = NumericDiffOptions())
198       : functor_(functor),
199         ownership_(ownership),
200         options_(options) {
201     if (kNumResiduals == DYNAMIC) {
202       SizedCostFunction<kNumResiduals,
203                         N0, N1, N2, N3, N4,
204                         N5, N6, N7, N8, N9>
205           ::set_num_residuals(num_residuals);
206     }
207   }
208
209   // Deprecated. New users should avoid using this constructor. Instead, use the
210   // constructor with NumericDiffOptions.
211   NumericDiffCostFunction(CostFunctor* functor,
212                           Ownership ownership,
213                           int num_residuals,
214                           const double relative_step_size)
215       :functor_(functor),
216        ownership_(ownership),
217        options_() {
218     LOG(WARNING) << "This constructor is deprecated and will be removed in "
219                     "a future version. Please use the NumericDiffOptions "
220                     "constructor instead.";
221
222     if (kNumResiduals == DYNAMIC) {
223       SizedCostFunction<kNumResiduals,
224                         N0, N1, N2, N3, N4,
225                         N5, N6, N7, N8, N9>
226           ::set_num_residuals(num_residuals);
227     }
228
229     options_.relative_step_size = relative_step_size;
230   }
231
232   ~NumericDiffCostFunction() {
233     if (ownership_ != TAKE_OWNERSHIP) {
234       functor_.release();
235     }
236   }
237
238   virtual bool Evaluate(double const* const* parameters,
239                         double* residuals,
240                         double** jacobians) const {
241     using internal::FixedArray;
242     using internal::NumericDiff;
243
244     const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
245     const int kNumParameterBlocks =
246         (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
247         (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
248
249     // Get the function value (residuals) at the the point to evaluate.
250     if (!internal::EvaluateImpl<CostFunctor,
251                                 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
252                                     functor_.get(),
253                                     parameters,
254                                     residuals,
255                                     functor_.get())) {
256       return false;
257     }
258
259     if (jacobians == NULL) {
260       return true;
261     }
262
263     // Create a copy of the parameters which will get mutated.
264     FixedArray<double> parameters_copy(kNumParameters);
265     FixedArray<double*> parameters_reference_copy(kNumParameterBlocks);
266
267     parameters_reference_copy[0] = parameters_copy.get();
268     if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0;
269     if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1;
270     if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2;
271     if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3;
272     if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4;
273     if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5;
274     if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6;
275     if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7;
276     if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8;
277
278 #define CERES_COPY_PARAMETER_BLOCK(block)                               \
279   if (N ## block) memcpy(parameters_reference_copy[block],              \
280                          parameters[block],                             \
281                          sizeof(double) * N ## block);  // NOLINT
282
283     CERES_COPY_PARAMETER_BLOCK(0);
284     CERES_COPY_PARAMETER_BLOCK(1);
285     CERES_COPY_PARAMETER_BLOCK(2);
286     CERES_COPY_PARAMETER_BLOCK(3);
287     CERES_COPY_PARAMETER_BLOCK(4);
288     CERES_COPY_PARAMETER_BLOCK(5);
289     CERES_COPY_PARAMETER_BLOCK(6);
290     CERES_COPY_PARAMETER_BLOCK(7);
291     CERES_COPY_PARAMETER_BLOCK(8);
292     CERES_COPY_PARAMETER_BLOCK(9);
293
294 #undef CERES_COPY_PARAMETER_BLOCK
295
296 #define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block)                        \
297     if (N ## block && jacobians[block] != NULL) {                       \
298       if (!NumericDiff<CostFunctor,                                     \
299                        method,                                          \
300                        kNumResiduals,                                   \
301                        N0, N1, N2, N3, N4, N5, N6, N7, N8, N9,          \
302                        block,                                           \
303                        N ## block >::EvaluateJacobianForParameterBlock( \
304                            functor_.get(),                              \
305                            residuals,                                   \
306                            options_,                                    \
307                           SizedCostFunction<kNumResiduals,              \
308                            N0, N1, N2, N3, N4,                          \
309                            N5, N6, N7, N8, N9>::num_residuals(),        \
310                            block,                                       \
311                            N ## block,                                  \
312                            parameters_reference_copy.get(),             \
313                            jacobians[block])) {                         \
314         return false;                                                   \
315       }                                                                 \
316     }
317
318     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0);
319     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1);
320     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2);
321     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3);
322     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4);
323     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5);
324     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6);
325     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7);
326     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8);
327     CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9);
328
329 #undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK
330
331     return true;
332   }
333
334  private:
335   internal::scoped_ptr<CostFunctor> functor_;
336   Ownership ownership_;
337   NumericDiffOptions options_;
338 };
339
340 }  // namespace ceres
341
342 #endif  // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_