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 (at) 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 // The autodiff cost function 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 6 independent 114 // variables, and there is no limit on the dimensionality of each of them. 115 // 116 // WARNING #1: Since the functor will get instantiated with different types for 117 // T, you must to convert from other numeric types to T before mixing 118 // computations with other variables of type T. In the example above, this is 119 // seen where instead of using k_ directly, k_ is wrapped with T(k_). 120 // 121 // WARNING #2: A common beginner's error when first using autodiff cost 122 // functions is to get the sizing wrong. In particular, there is a tendency to 123 // set the template parameters to (dimension of residual, number of parameters) 124 // instead of passing a dimension parameter for *every parameter*. In the 125 // example above, that would be <MyScalarCostFunctor, 1, 2>, which is missing 126 // the last '2' argument. Please be careful when setting the size parameters. 127 128 #ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ 129 #define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ 130 131 #include "ceres/internal/autodiff.h" 132 #include "ceres/internal/scoped_ptr.h" 133 #include "ceres/sized_cost_function.h" 134 #include "ceres/types.h" 135 #include "glog/logging.h" 136 137 namespace ceres { 138 139 // A cost function which computes the derivative of the cost with respect to 140 // the parameters (a.k.a. the jacobian) using an autodifferentiation framework. 141 // The first template argument is the functor object, described in the header 142 // comment. The second argument is the dimension of the residual (or 143 // ceres::DYNAMIC to indicate it will be set at runtime), and subsequent 144 // arguments describe the size of the Nth parameter, one per parameter. 145 // 146 // The constructors take ownership of the cost functor. 147 // 148 // If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the 149 // two-argument constructor must be used. The second constructor takes a number 150 // of residuals (in addition to the templated number of residuals). This allows 151 // for varying the number of residuals for a single autodiff cost function at 152 // runtime. 153 template <typename CostFunctor, 154 int M, // Number of residuals, or ceres::DYNAMIC. 155 int N0, // Number of parameters in block 0. 156 int N1 = 0, // Number of parameters in block 1. 157 int N2 = 0, // Number of parameters in block 2. 158 int N3 = 0, // Number of parameters in block 3. 159 int N4 = 0, // Number of parameters in block 4. 160 int N5 = 0, // Number of parameters in block 5. 161 int N6 = 0, // Number of parameters in block 6. 162 int N7 = 0, // Number of parameters in block 7. 163 int N8 = 0, // Number of parameters in block 8. 164 int N9 = 0> // Number of parameters in block 9. 165 class AutoDiffCostFunction : public SizedCostFunction<M, 166 N0, N1, N2, N3, N4, 167 N5, N6, N7, N8, N9> { 168 public: 169 // Takes ownership of functor. Uses the template-provided value for the 170 // number of residuals ("M"). 171 explicit AutoDiffCostFunction(CostFunctor* functor) 172 : functor_(functor) { 173 CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the " 174 << "number of residuals is set to ceres::DYNAMIC."; 175 } 176 177 // Takes ownership of functor. Ignores the template-provided number of 178 // residuals ("M") in favor of the "num_residuals" argument provided. 179 // 180 // This allows for having autodiff cost functions which return varying 181 // numbers of residuals at runtime. 182 AutoDiffCostFunction(CostFunctor* functor, int num_residuals) 183 : functor_(functor) { 184 CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the " 185 << "number of residuals is not ceres::DYNAMIC."; 186 SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> 187 ::set_num_residuals(num_residuals); 188 } 189 190 virtual ~AutoDiffCostFunction() {} 191 192 // Implementation details follow; clients of the autodiff cost function should 193 // not have to examine below here. 194 // 195 // To handle varardic cost functions, some template magic is needed. It's 196 // mostly hidden inside autodiff.h. 197 virtual bool Evaluate(double const* const* parameters, 198 double* residuals, 199 double** jacobians) const { 200 if (!jacobians) { 201 return internal::VariadicEvaluate< 202 CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> 203 ::Call(*functor_, parameters, residuals); 204 } 205 return internal::AutoDiff<CostFunctor, double, 206 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( 207 *functor_, 208 parameters, 209 SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> 210 ::num_residuals(), 211 residuals, 212 jacobians); 213 } 214 215 private: 216 internal::scoped_ptr<CostFunctor> functor_; 217 }; 218 219 } // namespace ceres 220 221 #endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ 222
