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/
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: sameeragarwal@google.com (Sameer Agarwal)
30//
31// Create CostFunctions as needed by the least squares framework, with
32// Jacobians computed via automatic differentiation. For more
33// information on automatic differentation, see the wikipedia article
34// at http://en.wikipedia.org/wiki/Automatic_differentiation
35//
36// To get an auto differentiated cost function, you must define a class with a
37// templated operator() (a functor) that computes the cost function in terms of
38// the template parameter T. The autodiff framework substitutes appropriate
39// "jet" objects for T in order to compute the derivative when necessary, but
40// this is hidden, and you should write the function as if T were a scalar type
41// (e.g. a double-precision floating point number).
42//
43// The function must write the computed value in the last argument
44// (the only non-const one) and return true to indicate
45// success. Please see cost_function.h for details on how the return
46// value maybe used to impose simple constraints on the parameter
47// block.
48//
49// For example, consider a scalar error e = k - x'y, where both x and y are
50// two-dimensional column vector parameters, the prime sign indicates
51// transposition, and k is a constant. The form of this error, which is the
52// difference between a constant and an expression, is a common pattern in least
53// squares problems. For example, the value x'y might be the model expectation
54// for a series of measurements, where there is an instance of the cost function
55// for each measurement k.
56//
57// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
58// the squaring is implicitly done by the optimization framework.
59//
60// To write an auto-differentiable cost function for the above model, first
61// define the object
62//
63//   class MyScalarCostFunctor {
64//     MyScalarCostFunctor(double k): k_(k) {}
65//
66//     template <typename T>
67//     bool operator()(const T* const x , const T* const y, T* e) const {
68//       e[0] = T(k_) - x[0] * y[0] + x[1] * y[1];
69//       return true;
70//     }
71//
72//    private:
73//     double k_;
74//   };
75//
76// Note that in the declaration of operator() the input parameters x and y come
77// first, and are passed as const pointers to arrays of T. If there were three
78// input parameters, then the third input parameter would come after y. The
79// output is always the last parameter, and is also a pointer to an array. In
80// the example above, e is a scalar, so only e[0] is set.
81//
82// Then given this class definition, the auto differentiated cost function for
83// it can be constructed as follows.
84//
85//   CostFunction* cost_function
86//       = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
87//            new MyScalarCostFunctor(1.0));             ^  ^  ^
88//                                                       |  |  |
89//                            Dimension of residual -----+  |  |
90//                            Dimension of x ---------------+  |
91//                            Dimension of y ------------------+
92//
93// In this example, there is usually an instance for each measumerent of k.
94//
95// In the instantiation above, the template parameters following
96// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a
97// 1-dimensional output from two arguments, both 2-dimensional.
98//
99// AutoDiffCostFunction also supports cost functions with a
100// runtime-determined number of residuals. For example:
101//
102//   CostFunction* cost_function
103//       = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
104//           new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
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// WARNING #1: Since the functor will get instantiated with different types for
118// T, you must to convert from other numeric types to T before mixing
119// computations with other variables of type T. In the example above, this is
120// seen where instead of using k_ directly, k_ is wrapped with T(k_).
121//
122// WARNING #2: A common beginner's error when first using autodiff cost
123// functions is to get the sizing wrong. In particular, there is a tendency to
124// set the template parameters to (dimension of residual, number of parameters)
125// instead of passing a dimension parameter for *every parameter*. In the
126// example above, that would be <MyScalarCostFunctor, 1, 2>, which is missing
127// the last '2' argument. Please be careful when setting the size parameters.
128
129#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
130#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
131
132#include "ceres/internal/autodiff.h"
133#include "ceres/internal/scoped_ptr.h"
134#include "ceres/sized_cost_function.h"
135#include "ceres/types.h"
136#include "glog/logging.h"
137
138namespace ceres {
139
140// A cost function which computes the derivative of the cost with respect to
141// the parameters (a.k.a. the jacobian) using an autodifferentiation framework.
142// The first template argument is the functor object, described in the header
143// comment. The second argument is the dimension of the residual (or
144// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent
145// arguments describe the size of the Nth parameter, one per parameter.
146//
147// The constructors take ownership of the cost functor.
148//
149// If the number of residuals (argument kNumResiduals below) is
150// ceres::DYNAMIC, then the two-argument constructor must be used. The
151// second constructor takes a number of residuals (in addition to the
152// templated number of residuals). This allows for varying the number
153// of residuals for a single autodiff cost function at runtime.
154template <typename CostFunctor,
155          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
156          int N0,       // Number of parameters in block 0.
157          int N1 = 0,   // Number of parameters in block 1.
158          int N2 = 0,   // Number of parameters in block 2.
159          int N3 = 0,   // Number of parameters in block 3.
160          int N4 = 0,   // Number of parameters in block 4.
161          int N5 = 0,   // Number of parameters in block 5.
162          int N6 = 0,   // Number of parameters in block 6.
163          int N7 = 0,   // Number of parameters in block 7.
164          int N8 = 0,   // Number of parameters in block 8.
165          int N9 = 0>   // Number of parameters in block 9.
166class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
167                                                      N0, N1, N2, N3, N4,
168                                                      N5, N6, N7, N8, N9> {
169 public:
170  // Takes ownership of functor. Uses the template-provided value for the
171  // number of residuals ("kNumResiduals").
172  explicit AutoDiffCostFunction(CostFunctor* functor)
173      : functor_(functor) {
174    CHECK_NE(kNumResiduals, DYNAMIC)
175        << "Can't run the fixed-size constructor if the "
176        << "number of residuals is set to ceres::DYNAMIC.";
177  }
178
179  // Takes ownership of functor. Ignores the template-provided
180  // kNumResiduals in favor of the "num_residuals" argument provided.
181  //
182  // This allows for having autodiff cost functions which return varying
183  // numbers of residuals at runtime.
184  AutoDiffCostFunction(CostFunctor* functor, int num_residuals)
185      : functor_(functor) {
186    CHECK_EQ(kNumResiduals, DYNAMIC)
187        << "Can't run the dynamic-size constructor if the "
188        << "number of residuals is not ceres::DYNAMIC.";
189    SizedCostFunction<kNumResiduals,
190                      N0, N1, N2, N3, N4,
191                      N5, N6, N7, N8, N9>
192        ::set_num_residuals(num_residuals);
193  }
194
195  virtual ~AutoDiffCostFunction() {}
196
197  // Implementation details follow; clients of the autodiff cost function should
198  // not have to examine below here.
199  //
200  // To handle varardic cost functions, some template magic is needed. It's
201  // mostly hidden inside autodiff.h.
202  virtual bool Evaluate(double const* const* parameters,
203                        double* residuals,
204                        double** jacobians) const {
205    if (!jacobians) {
206      return internal::VariadicEvaluate<
207          CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
208          ::Call(*functor_, parameters, residuals);
209    }
210    return internal::AutoDiff<CostFunctor, double,
211           N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
212               *functor_,
213               parameters,
214               SizedCostFunction<kNumResiduals,
215                                 N0, N1, N2, N3, N4,
216                                 N5, N6, N7, N8, N9>::num_residuals(),
217               residuals,
218               jacobians);
219  }
220
221 private:
222  internal::scoped_ptr<CostFunctor> functor_;
223};
224
225}  // namespace ceres
226
227#endif  // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
228