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 {