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