From 2fc0ed6143ad499d6dc82d621ff5ec69170beb52 Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Tue, 15 Jan 2013 11:34:10 -0800 Subject: [PATCH] Change NumericDiffCostFunction to accept variadic functors. The interface for NumericDiffCostFunction and AutoDiffCostFunction are not comparable. They both accept variadic functors. The change is backward compatible, as it still supports numeric differentiation of CostFunction objects. Some refactoring of documentation and code in auto_diff_cost_function and its relatives was also done to make things consistent. Change-Id: Ib5f230a1d4a85738eb187803b9c1cd7166bb3b92 --- include/ceres/autodiff_cost_function.h | 46 +-- include/ceres/internal/autodiff.h | 140 +------ include/ceres/internal/numeric_diff.h | 198 ++++++++++ include/ceres/internal/variadic_evaluate.h | 182 +++++++++ include/ceres/numeric_diff_cost_function.h | 372 +++++++++--------- include/ceres/types.h | 5 + .../ceres/numeric_diff_cost_function_test.cc | 104 +++-- 7 files changed, 668 insertions(+), 379 deletions(-) create mode 100644 include/ceres/internal/numeric_diff.h create mode 100644 include/ceres/internal/variadic_evaluate.h diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h index bb08d649e..2f86f56a6 100644 --- a/include/ceres/autodiff_cost_function.h +++ b/include/ceres/autodiff_cost_function.h @@ -28,10 +28,10 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // -// Helpers for making CostFunctions as needed by the least squares framework, -// with Jacobians computed via automatic differentiation. For more information -// on automatic differentation, see the wikipedia article at -// http://en.wikipedia.org/wiki/Automatic_differentiation +// Create CostFunctions as needed by the least squares framework, with +// Jacobians computed via automatic differentiation. For more +// information on automatic differentation, see the wikipedia article +// at http://en.wikipedia.org/wiki/Automatic_differentiation // // To get an auto differentiated cost function, you must define a class with a // templated operator() (a functor) that computes the cost function in terms of @@ -57,8 +57,8 @@ // To write an auto-differentiable cost function for the above model, first // define the object // -// class MyScalarCostFunction { -// MyScalarCostFunction(double k): k_(k) {} +// class MyScalarCostFunctor { +// MyScalarCostFunctor(double k): k_(k) {} // // template // bool operator()(const T* const x , const T* const y, T* e) const { @@ -80,32 +80,32 @@ // it can be constructed as follows. // // CostFunction* cost_function -// = new AutoDiffCostFunction( -// new MyScalarCostFunction(1.0)); ^ ^ ^ -// | | | -// Dimension of residual ------+ | | -// Dimension of x ----------------+ | -// Dimension of y -------------------+ +// = new AutoDiffCostFunction( +// new MyScalarCostFunctor(1.0)); ^ ^ ^ +// | | | +// 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 -// "MyScalarCostFunction", "1, 2, 2", describe the functor as computing a +// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a // 1-dimensional output from two arguments, both 2-dimensional. // // The autodiff cost function also supports cost functions with a // runtime-determined number of residuals. For example: // // CostFunction* cost_function -// = new AutoDiffCostFunction( -// new CostFunctionWithDynamicNumResiduals(1.0), ^ ^ ^ -// runtime_number_of_residuals); <----+ | | | -// | | | | -// | | | | -// Actual number of residuals ------+ | | | -// Indicate dynamic number of residuals ---------+ | | -// Dimension of x -------------------------------------+ | -// Dimension of y ----------------------------------------+ +// = new AutoDiffCostFunction( +// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals --------+ | | +// Dimension of x ------------------------------------+ | +// Dimension of y ---------------------------------------+ // // The framework can currently accommodate cost functions of up to 6 independent // variables, and there is no limit on the dimensionality of each of them. @@ -119,7 +119,7 @@ // functions 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 , which is missing +// example above, that would be , which is missing // the last '2' argument. Please be careful when setting the size parameters. #ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index 36e45ac3d..abc16e67a 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h @@ -146,6 +146,7 @@ #include "ceres/jet.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" +#include "ceres/internal/variadic_evaluate.h" namespace ceres { namespace internal { @@ -195,144 +196,6 @@ inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { } } -// This block of quasi-repeated code calls the user-supplied functor, which may -// take a variable number of arguments. This is accomplished by specializing the -// struct based on the size of the trailing parameters; parameters with 0 size -// are assumed missing. -// -// Supporting variadic functions is the primary source of complexity in the -// autodiff implementation. - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - input[8], - input[9], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - input[8], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - output); - } -}; - -template -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - output); - } -}; - // This is in a struct because default template parameters on a function are not // supported in C++03 (though it is available in C++0x). N0 through N5 are the // dimension of the input arguments to the user supplied functor. @@ -390,6 +253,7 @@ struct AutoDiff { x.get() + jet8, x.get() + jet9, }; + JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; #define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h new file mode 100644 index 000000000..1837df4af --- /dev/null +++ b/include/ceres/internal/numeric_diff.h @@ -0,0 +1,198 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) +// +// Finite differencing routine used by NumericDiffCostFunction. + +#ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ +#define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ + +#include +#include +#include "Eigen/Dense" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/cost_function.h" +#include "ceres/internal/variadic_evaluate.h" +#include "ceres/types.h" +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +// Helper templates that allow evaluation of a variadic functor or a +// CostFunction object. +template +bool EvaluateImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const void* /* NOT USED */) { + return VariadicEvaluate::Call( + *functor, + parameters, + residuals); +} + +template +bool EvaluateImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const CostFunction* /* NOT USED */) { + return functor->Evaluate(parameters, residuals, NULL); +} + +// 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 +struct NumericDiff { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctor* functor, + double const* residuals_at_eval_point, + const double relative_step_size, + double **parameters, + double *jacobian) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::RowMajor; + using Eigen::ColMajor; + + typedef Matrix ResidualVector; + typedef Matrix ParameterVector; + typedef Matrix 1) ? ColMajor : RowMajor> JacobianMatrix; + + + Map parameter_jacobian(jacobian, + kNumResiduals, + kParameterBlockSize); + + // Mutate 1 element at a time and then restore. + Map x_plus_delta(parameters[kParameterBlock], + kParameterBlockSize); + ParameterVector x(x_plus_delta); + ParameterVector step_size = x.array().abs() * relative_step_size; + + // To handle cases where a parameter is exactly zero, instead use + // the mean step_size for the other dimensions. If all the + // parameters are zero, there's no good answer. Take + // relative_step_size as a guess and hope for the best. + const double fallback_step_size = + (step_size.sum() == 0) + ? relative_step_size + : step_size.sum() / step_size.rows(); + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + for (int j = 0; j < kParameterBlockSize; ++j) { + const double delta = + (step_size(j) == 0.0) ? fallback_step_size : step_size(j); + + x_plus_delta(j) = x(j) + delta; + + double residuals[kNumResiduals]; // NOLINT + + if (!EvaluateImpl( + functor, parameters, residuals, functor)) { + 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(residuals, kNumResiduals); + + double one_over_delta = 1.0 / delta; + if (kMethod == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - delta; + + if (!EvaluateImpl( + functor, parameters, residuals, functor)) { + return false; + } + + parameter_jacobian.col(j) -= + Map(residuals, kNumResiduals, 1); + one_over_delta /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map(residuals_at_eval_point, kNumResiduals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_delta; + } + return true; + } +}; + +template +struct NumericDiff { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctor* functor, + double const* residuals_at_eval_point, + const double relative_step_size, + double **parameters, + double *jacobian) { + LOG(FATAL) << "Control should never reach here."; + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h new file mode 100644 index 000000000..4b1e4bdc6 --- /dev/null +++ b/include/ceres/internal/variadic_evaluate.h @@ -0,0 +1,182 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ +#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ + +#include + +#include +#include "ceres/jet.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" + +namespace ceres { +namespace internal { + +// This block of quasi-repeated code calls the user-supplied functor, which may +// take a variable number of arguments. This is accomplished by specializing the +// struct based on the size of the trailing parameters; parameters with 0 size +// are assumed missing. +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + input[9], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + output); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h index 8544e44d0..54ba26fd8 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h @@ -27,19 +27,109 @@ // 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 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. +// 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( +// 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 , 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: @@ -51,233 +141,151 @@ // 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 #include #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 { -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 -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; - using Eigen::ColMajor; - - typedef Matrix ResidualVector; - typedef Matrix ParameterVector; - typedef Matrix 1) ? ColMajor : RowMajor> JacobianMatrix; - - Map parameter_jacobian(jacobians[parameter_block], - num_residuals, - parameter_block_size); - - // Mutate 1 element at a time and then restore. - Map 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(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(residuals, num_residuals, 1); - one_over_h /= 2; - } else { - // Forward difference only; reuse existing residuals evaluation. - parameter_jacobian.col(j) -= - Map(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 -struct Differencer { - 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 +template // Number of parameters in block 9. class NumericDiffCostFunction - : public SizedCostFunction { + : public SizedCostFunction { public: - NumericDiffCostFunction(CostFunctionNoJacobian* function, - Ownership ownership) - : function_(function), ownership_(ownership) {} + NumericDiffCostFunction(CostFunctor* functor, + const double relative_step_size = 1e-6) + :functor_(functor), + ownership_(TAKE_OWNERSHIP), + relative_step_size_(relative_step_size) {} - virtual ~NumericDiffCostFunction() { + 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) { - function_.release(); + 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. - bool success = function_->Evaluate(parameters, residuals, NULL); - if (!success) { - // Something went wrong; ignore the jacobian. + if (!internal::EvaluateImpl( + functor_.get(), + parameters, + residuals, + functor_.get())) { 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; + FixedArray parameters_copy(kNumParameters); + FixedArray 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 -#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); + 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]) { \ - if (!Differencer::EvaluateJacobianForParameterBlock( \ - function_.get(), \ - residuals, \ - parameters_references_copy, \ - jacobians)) { \ - return false; \ - } \ +#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \ + if (N ## block && jacobians[block] != NULL) { \ + if (!NumericDiff::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 function_; + internal::scoped_ptr functor_; Ownership ownership_; + const double relative_step_size_; }; } // namespace ceres diff --git a/include/ceres/types.h b/include/ceres/types.h index edf404ae3..2c0f0e5a8 100644 --- a/include/ceres/types.h +++ b/include/ceres/types.h @@ -335,6 +335,11 @@ enum DimensionType { DYNAMIC = -1 }; +enum NumericDiffMethod { + CENTRAL, + FORWARD +}; + const char* LinearSolverTypeToString(LinearSolverType type); bool StringToLinearSolverType(string value, LinearSolverType* type); diff --git a/internal/ceres/numeric_diff_cost_function_test.cc b/internal/ceres/numeric_diff_cost_function_test.cc index df12eb98d..296cc22ec 100644 --- a/internal/ceres/numeric_diff_cost_function_test.cc +++ b/internal/ceres/numeric_diff_cost_function_test.cc @@ -34,7 +34,6 @@ #include #include #include -#include "ceres/cost_function.h" #include "ceres/internal/macros.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/sized_cost_function.h" @@ -50,48 +49,65 @@ namespace internal { // y1 = x1'x2 -> dy1/dx1 = x2, dy1/dx2 = x1 // y2 = (x1'x2)^2 -> dy2/dx1 = 2 * x2 * (x1'x2), dy2/dx2 = 2 * x1 * (x1'x2) // y3 = x2'x2 -> dy3/dx1 = 0, dy3/dx2 = 2 * x2 -class TestCostFunction : public CostFunction { - public: - TestCostFunction() { - set_num_residuals(3); - mutable_parameter_block_sizes()->push_back(5); // x1. - mutable_parameter_block_sizes()->push_back(5); // x2. - } - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - (void) jacobians; // Ignored. - +struct EasyFunctor { + bool operator()(const double* x1, const double* x2, double* residuals) const { residuals[0] = residuals[1] = residuals[2] = 0; for (int i = 0; i < 5; ++i) { - residuals[0] += parameters[0][i] * parameters[1][i]; - residuals[2] += parameters[1][i] * parameters[1][i]; + residuals[0] += x1[i] * x2[i]; + residuals[2] += x2[i] * x2[i]; } residuals[1] = residuals[0] * residuals[0]; return true; } }; +class EasyCostFunction : public SizedCostFunction<3, 5, 5> { + public: + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + (void) jacobians; // Ignored. + return EasyFunctor()(parameters[0], parameters[1], residuals); + } +}; + TEST(NumericDiffCostFunction, EasyCase) { // Try both central and forward difference. - internal::scoped_ptr cfs[2]; + internal::scoped_ptr cfs[4]; cfs[0].reset( - new NumericDiffCostFunction( - new TestCostFunction, TAKE_OWNERSHIP)); + new EasyCostFunction, TAKE_OWNERSHIP)); cfs[1].reset( - new NumericDiffCostFunction( - new TestCostFunction, TAKE_OWNERSHIP)); + new EasyCostFunction, TAKE_OWNERSHIP)); - for (int c = 0; c < 2; ++c) { + cfs[2].reset( + new NumericDiffCostFunction< EasyFunctor, + CENTRAL, + 3, /* number of residuals */ + 5, /* size of x1 */ + 5 /* size of x2 */>( + new EasyFunctor)); + + cfs[3].reset( + new NumericDiffCostFunction< EasyFunctor, + FORWARD, + 3, /* number of residuals */ + 5, /* size of x1 */ + 5 /* size of x2 */>( + new EasyFunctor)); + + + for (int c = 0; c < 4; ++c) { CostFunction *cost_function = cfs[c].get(); double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 }; @@ -131,21 +147,11 @@ TEST(NumericDiffCostFunction, EasyCase) { // // dy1/dx1 = x2 * cos(x1'x2), dy1/dx2 = x1 * cos(x1'x2) // dy2/dx1 = -x2 * exp(-x1'x2 / 10) / 10, dy2/dx2 = -x2 * exp(-x1'x2 / 10) / 10 -class TranscendentalTestCostFunction : public CostFunction { - public: - TranscendentalTestCostFunction() { - set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(5); // x1. - mutable_parameter_block_sizes()->push_back(5); // x2. - } - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - (void) jacobians; // Ignored. - +struct TranscendentalFunctor { + bool operator()(const double* x1, const double* x2, double* residuals) const { double x1x2 = 0; for (int i = 0; i < 5; ++i) { - x1x2 += parameters[0][i] * parameters[1][i]; + x1x2 += x1[i] * x2[i]; } residuals[0] = sin(x1x2); residuals[1] = exp(-x1x2 / 10); @@ -153,9 +159,19 @@ class TranscendentalTestCostFunction : public CostFunction { } }; +class TranscendentalTestCostFunction : public SizedCostFunction<2, 5, 5> { + public: + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + (void) jacobians; // Ignored. + return TranscendentalFunctor()(parameters[0], parameters[1], residuals); + } +}; + TEST(NumericDiffCostFunction, TransendentalOperationsInCostFunction) { // Try both central and forward difference. - internal::scoped_ptr cfs[2]; + internal::scoped_ptr cfs[4]; cfs[0].reset( new NumericDiffCostFunction( new TranscendentalTestCostFunction, TAKE_OWNERSHIP)); - for (int c = 0; c < 2; ++c) { + cfs[2].reset( + new NumericDiffCostFunction( + new TranscendentalFunctor)); + + cfs[3].reset( + new NumericDiffCostFunction( + new TranscendentalFunctor)); + + for (int c = 0; c < 4; ++c) { CostFunction *cost_function = cfs[c].get(); struct {