| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283 | // 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)//// 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 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.//// 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(keir): Characterize accuracy; mention pitfalls; provide alternatives.#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_#include <cstring>#include <glog/logging.h>#include "Eigen/Dense"#include "ceres/internal/scoped_ptr.h"#include "ceres/sized_cost_function.h"#include "ceres/types.h"namespace ceres {enum NumericDiffMethod {  CENTRAL,  FORWARD};// This is split from the main class because C++ doesn't allow partial template// specializations for member functions. The alternative is to repeat the main// class for differing numbers of parameters, which is also unfortunate.template <typename CostFunctionNoJacobian,          int num_residuals,          int parameter_block_size,          int parameter_block,          NumericDiffMethod method>struct Differencer {  // Mutates parameters but must restore them before return.  static bool EvaluateJacobianForParameterBlock(      const CostFunctionNoJacobian *function,      double const* residuals_at_eval_point,      double **parameters,      double **jacobians) {    using Eigen::Map;    using Eigen::Matrix;    using Eigen::RowMajor;    typedef Matrix<double, num_residuals, 1> ResidualVector;    typedef Matrix<double, parameter_block_size, 1> ParameterVector;    typedef Matrix<double, num_residuals, parameter_block_size, RowMajor>        JacobianMatrix;    Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block],                                           num_residuals,                                           parameter_block_size);    // Mutate 1 element at a time and then restore.    Map<ParameterVector> x_plus_delta(parameters[parameter_block],                                      parameter_block_size);    ParameterVector x(x_plus_delta);    // TODO(keir): Pick a smarter number! In theory a good choice is sqrt(eps) *    // x, which for doubles means about 1e-8 * x. However, I have found this    // number too optimistic. This number should be exposed for users to change.    const double kRelativeStepSize = 1e-6;    ParameterVector step_size = x.array().abs() * kRelativeStepSize;    // To handle cases where a parameter is exactly zero, instead use the mean    // step_size for the other dimensions.    double fallback_step_size = step_size.sum() / step_size.rows();    if (fallback_step_size == 0.0) {      // If all the parameters are zero, there's no good answer. Take      // kRelativeStepSize as a guess and hope for the best.      fallback_step_size = kRelativeStepSize;    }    // For each parameter in the parameter block, use finite differences to    // compute the derivative for that parameter.    for (int j = 0; j < parameter_block_size; ++j) {      if (step_size(j) == 0.0) {        // The parameter is exactly zero, so compromise and use the mean        // step_size from the other parameters. This can break in many cases,        // but it's hard to pick a good number without problem specific        // knowledge.        step_size(j) = fallback_step_size;      }      x_plus_delta(j) = x(j) + step_size(j);      double residuals[num_residuals];  // NOLINT      if (!function->Evaluate(parameters, residuals, NULL)) {        // Something went wrong; bail.        return false;      }      // Compute this column of the jacobian in 3 steps:      // 1. Store residuals for the forward part.      // 2. Subtract residuals for the backward (or 0) part.      // 3. Divide out the run.      parameter_jacobian.col(j) =          Map<const ResidualVector>(residuals, num_residuals);      double one_over_h = 1 / step_size(j);      if (method == CENTRAL) {        // Compute the function on the other side of x(j).        x_plus_delta(j) = x(j) - step_size(j);        if (!function->Evaluate(parameters, residuals, NULL)) {          // Something went wrong; bail.          return false;        }        parameter_jacobian.col(j) -=            Map<ResidualVector>(residuals, num_residuals, 1);        one_over_h /= 2;      } else {        // Forward difference only; reuse existing residuals evaluation.        parameter_jacobian.col(j) -=            Map<const ResidualVector>(residuals_at_eval_point, num_residuals);      }      x_plus_delta(j) = x(j);  // Restore x_plus_delta.      // Divide out the run to get slope.      parameter_jacobian.col(j) *= one_over_h;    }    return true;  }};// Prevent invalid instantiations.template <typename CostFunctionNoJacobian,          int num_residuals,          int parameter_block,          NumericDiffMethod method>struct Differencer<CostFunctionNoJacobian,                  num_residuals,                  0 /* parameter_block_size */,                  parameter_block,                  method> {  static bool EvaluateJacobianForParameterBlock(      const CostFunctionNoJacobian *function,      double const* residuals_at_eval_point,      double **parameters,      double **jacobians) {    LOG(FATAL) << "Shouldn't get here.";    return true;  }};template <typename CostFunctionNoJacobian,         NumericDiffMethod method = CENTRAL, int M = 0,         int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0>class NumericDiffCostFunction    : public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> { public:  NumericDiffCostFunction(CostFunctionNoJacobian* function,                          Ownership ownership)      : function_(function), ownership_(ownership) {}  virtual ~NumericDiffCostFunction() {    if (ownership_ != TAKE_OWNERSHIP) {      function_.release();    }  }  virtual bool Evaluate(double const* const* parameters,                        double* residuals,                        double** jacobians) const {    // Get the function value (residuals) at the the point to evaluate.    bool success = function_->Evaluate(parameters, residuals, NULL);    if (!success) {      // Something went wrong; ignore the jacobian.      return false;    }    if (!jacobians) {      // Nothing to do; just forward.      return true;    }    // Create a copy of the parameters which will get mutated.    const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5;    double parameters_copy[kParametersSize];    double *parameters_references_copy[6];    parameters_references_copy[0] = ¶meters_copy[0];    parameters_references_copy[1] = ¶meters_copy[0] + N0;    parameters_references_copy[2] = ¶meters_copy[0] + N0 + N1;    parameters_references_copy[3] = ¶meters_copy[0] + N0 + N1 + N2;    parameters_references_copy[4] = ¶meters_copy[0] + N0 + N1 + N2 + N3;    parameters_references_copy[5] =        ¶meters_copy[0] + N0 + N1 + N2 + N3 + N4;#define COPY_PARAMETER_BLOCK(block) \    if (N ## block) memcpy(parameters_references_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);#undef COPY_PARAMETER_BLOCK#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \    if (N ## block && jacobians[block]) { \      if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \                       M, \                       N ## block, \                       block, \                       method>::EvaluateJacobianForParameterBlock( \          function_.get(), \          residuals, \          parameters_references_copy, \          jacobians)) { \        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);#undef EVALUATE_JACOBIAN_FOR_BLOCK    return true;  } private:  internal::scoped_ptr<CostFunctionNoJacobian> function_;  Ownership ownership_;};}  // namespace ceres#endif  // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
 |