|  | // Ceres Solver - A fast non-linear least squares minimizer | 
|  | // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. | 
|  | // http://code.google.com/p/ceres-solver/ | 
|  | // | 
|  | // Redistribution and use in source and binary forms, with or without | 
|  | // modification, are permitted provided that the following conditions are met: | 
|  | // | 
|  | // * Redistributions of source code must retain the above copyright notice, | 
|  | //   this list of conditions and the following disclaimer. | 
|  | // * Redistributions in binary form must reproduce the above copyright notice, | 
|  | //   this list of conditions and the following disclaimer in the documentation | 
|  | //   and/or other materials provided with the distribution. | 
|  | // * Neither the name of Google Inc. nor the names of its contributors may be | 
|  | //   used to endorse or promote products derived from this software without | 
|  | //   specific prior written permission. | 
|  | // | 
|  | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | 
|  | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | 
|  | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | 
|  | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | 
|  | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | 
|  | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | 
|  | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | 
|  | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | 
|  | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | 
|  | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | 
|  | // POSSIBILITY OF SUCH DAMAGE. | 
|  | // | 
|  | // Author: keir@google.com (Keir Mierle) | 
|  | //         sameeragarwal@google.com (Sameer Agarwal) | 
|  | // | 
|  | // Create CostFunctions as needed by the least squares framework with jacobians | 
|  | // computed via numeric (a.k.a. finite) differentiation. For more details see | 
|  | // http://en.wikipedia.org/wiki/Numerical_differentiation. | 
|  | // | 
|  | // To get an numerically differentiated cost function, you must define | 
|  | // a class with a operator() (a functor) that computes the residuals. | 
|  | // | 
|  | // The function must write the computed value in the last argument (the only | 
|  | // non-const one) and return true to indicate success. | 
|  | // | 
|  | // For example, consider a scalar error e = k - x'y, where both x and y are | 
|  | // two-dimensional column vector parameters, the prime sign indicates | 
|  | // transposition, and k is a constant. The form of this error, which is the | 
|  | // difference between a constant and an expression, is a common pattern in least | 
|  | // squares problems. For example, the value x'y might be the model expectation | 
|  | // for a series of measurements, where there is an instance of the cost function | 
|  | // for each measurement k. | 
|  | // | 
|  | // The actual cost added to the total problem is e^2, or (k - x'k)^2; however, | 
|  | // the squaring is implicitly done by the optimization framework. | 
|  | // | 
|  | // To write an numerically-differentiable cost function for the above model, first | 
|  | // define the object | 
|  | // | 
|  | //   class MyScalarCostFunctor { | 
|  | //     MyScalarCostFunctor(double k): k_(k) {} | 
|  | // | 
|  | //     bool operator()(const double* const x, | 
|  | //                     const double* const y, | 
|  | //                     double* residuals) const { | 
|  | //       residuals[0] = k_ - x[0] * y[0] + x[1] * y[1]; | 
|  | //       return true; | 
|  | //     } | 
|  | // | 
|  | //    private: | 
|  | //     double k_; | 
|  | //   }; | 
|  | // | 
|  | // Note that in the declaration of operator() the input parameters x | 
|  | // and y come first, and are passed as const pointers to arrays of | 
|  | // doubles. If there were three input parameters, then the third input | 
|  | // parameter would come after y. The output is always the last | 
|  | // parameter, and is also a pointer to an array. In the example above, | 
|  | // the residual is a scalar, so only residuals[0] is set. | 
|  | // | 
|  | // Then given this class definition, the numerically differentiated | 
|  | // cost function with central differences used for computing the | 
|  | // derivative can be constructed as follows. | 
|  | // | 
|  | //   CostFunction* cost_function | 
|  | //       = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>( | 
|  | //           new MyScalarCostFunctor(1.0));                          ^  ^  ^ | 
|  | //                                                               |   |  |  | | 
|  | //                                   Finite Differencing Scheme -+   |  |  | | 
|  | //                                   Dimension of residual ----------+  |  | | 
|  | //                                   Dimension of x --------------------+  | | 
|  | //                                   Dimension of y -----------------------+ | 
|  | // | 
|  | // In this example, there is usually an instance for each measumerent of k. | 
|  | // | 
|  | // In the instantiation above, the template parameters following | 
|  | // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing | 
|  | // a 1-dimensional output from two arguments, both 2-dimensional. | 
|  | // | 
|  | // The framework can currently accommodate cost functions of up to 10 | 
|  | // independent variables, and there is no limit on the dimensionality | 
|  | // of each of them. | 
|  | // | 
|  | // The central difference method is considerably more accurate at the cost of | 
|  | // twice as many function evaluations than forward difference. Consider using | 
|  | // central differences begin with, and only after that works, trying forward | 
|  | // difference to improve performance. | 
|  | // | 
|  | // TODO(sameeragarwal): Add support for dynamic number of residuals. | 
|  | // | 
|  | // WARNING #1: A common beginner's error when first using | 
|  | // NumericDiffCostFunction is to get the sizing wrong. In particular, | 
|  | // there is a tendency to set the template parameters to (dimension of | 
|  | // residual, number of parameters) instead of passing a dimension | 
|  | // parameter for *every parameter*. In the example above, that would | 
|  | // be <MyScalarCostFunctor, 1, 2>, which is missing the last '2' | 
|  | // argument. Please be careful when setting the size parameters. | 
|  | // | 
|  | //////////////////////////////////////////////////////////////////////////// | 
|  | //////////////////////////////////////////////////////////////////////////// | 
|  | // | 
|  | // ALTERNATE INTERFACE | 
|  | // | 
|  | // For a variety of reason, including compatibility with legacy code, | 
|  | // NumericDiffCostFunction can also take CostFunction objects as | 
|  | // input. The following describes how. | 
|  | // | 
|  | // To get a numerically differentiated cost function, define a | 
|  | // subclass of CostFunction such that the Evaluate() function ignores | 
|  | // the jacobian parameter. The numeric differentiation wrapper will | 
|  | // fill in the jacobian parameter if nececssary by repeatedly calling | 
|  | // the Evaluate() function with small changes to the appropriate | 
|  | // parameters, and computing the slope. For performance, the numeric | 
|  | // differentiation wrapper class is templated on the concrete cost | 
|  | // function, even though it could be implemented only in terms of the | 
|  | // virtual CostFunction interface. | 
|  | // | 
|  | // The numerically differentiated version of a cost function for a cost function | 
|  | // can be constructed as follows: | 
|  | // | 
|  | //   CostFunction* cost_function | 
|  | //       = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>( | 
|  | //           new MyCostFunction(...), TAKE_OWNERSHIP); | 
|  | // | 
|  | // where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8 | 
|  | // respectively. Look at the tests for a more detailed example. | 
|  | // | 
|  | // TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. | 
|  |  | 
|  | #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ | 
|  | #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ | 
|  |  | 
|  | #include <glog/logging.h> | 
|  | #include "Eigen/Dense" | 
|  | #include "ceres/cost_function.h" | 
|  | #include "ceres/internal/numeric_diff.h" | 
|  | #include "ceres/internal/scoped_ptr.h" | 
|  | #include "ceres/sized_cost_function.h" | 
|  | #include "ceres/types.h" | 
|  |  | 
|  | namespace ceres { | 
|  |  | 
|  | template <typename CostFunctor, | 
|  | NumericDiffMethod method = CENTRAL, | 
|  | int kNumResiduals = 0,  // Number of residuals, or ceres::DYNAMIC | 
|  | int N0 = 0,   // Number of parameters in block 0. | 
|  | int N1 = 0,   // Number of parameters in block 1. | 
|  | int N2 = 0,   // Number of parameters in block 2. | 
|  | int N3 = 0,   // Number of parameters in block 3. | 
|  | int N4 = 0,   // Number of parameters in block 4. | 
|  | int N5 = 0,   // Number of parameters in block 5. | 
|  | int N6 = 0,   // Number of parameters in block 6. | 
|  | int N7 = 0,   // Number of parameters in block 7. | 
|  | int N8 = 0,   // Number of parameters in block 8. | 
|  | int N9 = 0>   // Number of parameters in block 9. | 
|  | class NumericDiffCostFunction | 
|  | : public SizedCostFunction<kNumResiduals, | 
|  | N0, N1, N2, N3, N4, | 
|  | N5, N6, N7, N8, N9> { | 
|  | public: | 
|  | NumericDiffCostFunction(CostFunctor* functor, | 
|  | const double relative_step_size = 1e-6) | 
|  | :functor_(functor), | 
|  | ownership_(TAKE_OWNERSHIP), | 
|  | relative_step_size_(relative_step_size) {} | 
|  |  | 
|  | NumericDiffCostFunction(CostFunctor* functor, | 
|  | Ownership ownership, | 
|  | const double relative_step_size = 1e-6) | 
|  | : functor_(functor), | 
|  | ownership_(ownership), | 
|  | relative_step_size_(relative_step_size) {} | 
|  |  | 
|  | ~NumericDiffCostFunction() { | 
|  | if (ownership_ != TAKE_OWNERSHIP) { | 
|  | functor_.release(); | 
|  | } | 
|  | } | 
|  |  | 
|  | virtual bool Evaluate(double const* const* parameters, | 
|  | double* residuals, | 
|  | double** jacobians) const { | 
|  | using internal::FixedArray; | 
|  | using internal::NumericDiff; | 
|  |  | 
|  | const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; | 
|  | const int kNumParameterBlocks = | 
|  | (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + | 
|  | (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0); | 
|  |  | 
|  | // Get the function value (residuals) at the the point to evaluate. | 
|  | if (!internal::EvaluateImpl<CostFunctor, | 
|  | N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( | 
|  | functor_.get(), | 
|  | parameters, | 
|  | residuals, | 
|  | functor_.get())) { | 
|  | return false; | 
|  | } | 
|  |  | 
|  | if (!jacobians) { | 
|  | return true; | 
|  | } | 
|  |  | 
|  | // Create a copy of the parameters which will get mutated. | 
|  | FixedArray<double> parameters_copy(kNumParameters); | 
|  | FixedArray<double*> parameters_reference_copy(kNumParameterBlocks); | 
|  |  | 
|  | parameters_reference_copy[0] = parameters_copy.get(); | 
|  | if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0; | 
|  | if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1; | 
|  | if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2; | 
|  | if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3; | 
|  | if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4; | 
|  | if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5; | 
|  | if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6; | 
|  | if (N7) parameters_reference_copy[8] = parameters_reference_copy[7] + N7; | 
|  | if (N8) parameters_reference_copy[9] = parameters_reference_copy[8] + N8; | 
|  |  | 
|  | #define COPY_PARAMETER_BLOCK(block)                                     \ | 
|  | if (N ## block) memcpy(parameters_reference_copy[block],              \ | 
|  | parameters[block],                             \ | 
|  | sizeof(double) * N ## block);  // NOLINT | 
|  |  | 
|  | COPY_PARAMETER_BLOCK(0); | 
|  | COPY_PARAMETER_BLOCK(1); | 
|  | COPY_PARAMETER_BLOCK(2); | 
|  | COPY_PARAMETER_BLOCK(3); | 
|  | COPY_PARAMETER_BLOCK(4); | 
|  | COPY_PARAMETER_BLOCK(5); | 
|  | COPY_PARAMETER_BLOCK(6); | 
|  | COPY_PARAMETER_BLOCK(7); | 
|  | COPY_PARAMETER_BLOCK(8); | 
|  | COPY_PARAMETER_BLOCK(9); | 
|  |  | 
|  | #undef COPY_PARAMETER_BLOCK | 
|  |  | 
|  | #define EVALUATE_JACOBIAN_FOR_BLOCK(block)                              \ | 
|  | if (N ## block && jacobians[block] != NULL) {                       \ | 
|  | if (!NumericDiff<CostFunctor,                                     \ | 
|  | method,                                          \ | 
|  | kNumResiduals,                                   \ | 
|  | N0, N1, N2, N3, N4, N5, N6, N7, N8, N9,          \ | 
|  | block,                                           \ | 
|  | N ## block >::EvaluateJacobianForParameterBlock( \ | 
|  | functor_.get(),                              \ | 
|  | residuals,                                   \ | 
|  | relative_step_size_,                         \ | 
|  | parameters_reference_copy.get(),             \ | 
|  | jacobians[block])) {                         \ | 
|  | return false;                                                   \ | 
|  | }                                                                 \ | 
|  | } | 
|  |  | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(0); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(1); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(2); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(3); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(4); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(5); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(6); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(7); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(8); | 
|  | EVALUATE_JACOBIAN_FOR_BLOCK(9); | 
|  |  | 
|  | #undef EVALUATE_JACOBIAN_FOR_BLOCK | 
|  |  | 
|  | return true; | 
|  | } | 
|  |  | 
|  | private: | 
|  | internal::scoped_ptr<CostFunctor> functor_; | 
|  | Ownership ownership_; | 
|  | const double relative_step_size_; | 
|  | }; | 
|  |  | 
|  | }  // namespace ceres | 
|  |  | 
|  | #endif  // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ |