NumericDiffCostFunction supports dynamic number of residuals. 1. Update AutoDiffCostFunction template parameters to be consistent with NumericDiffCostFunction. 2. Update the documentation for NumericDiffCostFunction and AutoDiffCostFunction. Change-Id: I113038abb5bedebb0f6f326f2a4ac31480d785fc
diff --git a/docs/source/building.rst b/docs/source/building.rst index c326fd1..b74a9dd 100644 --- a/docs/source/building.rst +++ b/docs/source/building.rst
@@ -1,8 +1,8 @@ .. _chapter-building: -===================== -Building Ceres Solver -===================== +============ +Installation +============ Stable Ceres Solver releases are available for download at `code.google.com <http://code.google.com/p/ceres-solver/>`_. For the
diff --git a/docs/source/modeling.rst b/docs/source/modeling.rst index 8e6de12..a5e875d 100644 --- a/docs/source/modeling.rst +++ b/docs/source/modeling.rst
@@ -164,7 +164,7 @@ .. code-block:: c++ template <typename CostFunctor, - int M, // Number of residuals, or ceres::DYNAMIC. + int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. int N0, // Number of parameters in block 0. int N1 = 0, // Number of parameters in block 1. int N2 = 0, // Number of parameters in block 2. @@ -176,7 +176,7 @@ int N8 = 0, // Number of parameters in block 8. int N9 = 0> // Number of parameters in block 9. class AutoDiffCostFunction : public - SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { + SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { }; To get an auto differentiated cost function, you must define a @@ -254,6 +254,22 @@ computing a 1-dimensional output from two arguments, both 2-dimensional. + :class:`AutoDiffCostFunction` also supports cost functions with a + runtime-determined number of residuals. For example: + + .. code-block:: c++ + + CostFunction* cost_function + = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>( + 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 10 independent variables, and there is no limit on the dimensionality of each of them. @@ -342,12 +358,21 @@ .. code-block:: c++ - 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, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> + template <typename CostFunctor, + NumericDiffMethod method = CENTRAL, + int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. + int N0, // 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<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { + : public SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { }; To get a numerically differentiated :class:`CostFunction`, you must @@ -426,6 +451,24 @@ computing a 1-dimensional output from two arguments, both 2-dimensional. + NumericDiffCostFunction also supports cost functions with a + runtime-determined number of residuals. For example: + + .. code-block:: c++ + + CostFunction* cost_function + = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>( + new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ + TAKE_OWNERSHIP, | | | + 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 10 independent variables, and there is no limit on the dimensionality of each of them.
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h index 371a11f..cb6801f 100644 --- a/include/ceres/autodiff_cost_function.h +++ b/include/ceres/autodiff_cost_function.h
@@ -96,7 +96,7 @@ // "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 +// AutoDiffCostFunction also supports cost functions with a // runtime-determined number of residuals. For example: // // CostFunction* cost_function @@ -110,8 +110,9 @@ // 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. +// 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. // // WARNING #1: Since the functor will get instantiated with different types for // T, you must to convert from other numeric types to T before mixing @@ -145,13 +146,13 @@ // // The constructors take ownership of the cost functor. // -// If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the -// two-argument constructor must be used. The second constructor takes a number -// of residuals (in addition to the templated number of residuals). This allows -// for varying the number of residuals for a single autodiff cost function at -// runtime. +// If the number of residuals (argument kNumResiduals below) is +// ceres::DYNAMIC, then the two-argument constructor must be used. The +// second constructor takes a number of residuals (in addition to the +// templated number of residuals). This allows for varying the number +// of residuals for a single autodiff cost function at runtime. template <typename CostFunctor, - int M, // Number of residuals, or ceres::DYNAMIC. + int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. int N0, // Number of parameters in block 0. int N1 = 0, // Number of parameters in block 1. int N2 = 0, // Number of parameters in block 2. @@ -162,28 +163,30 @@ 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 AutoDiffCostFunction : public SizedCostFunction<M, +class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { public: // Takes ownership of functor. Uses the template-provided value for the - // number of residuals ("M"). + // number of residuals ("kNumResiduals"). explicit AutoDiffCostFunction(CostFunctor* functor) : functor_(functor) { - CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the " - << "number of residuals is set to ceres::DYNAMIC."; + CHECK_NE(kNumResiduals, DYNAMIC) + << "Can't run the fixed-size constructor if the " + << "number of residuals is set to ceres::DYNAMIC."; } - // Takes ownership of functor. Ignores the template-provided number of - // residuals ("M") in favor of the "num_residuals" argument provided. + // Takes ownership of functor. Ignores the template-provided + // kNumResiduals in favor of the "num_residuals" argument provided. // // This allows for having autodiff cost functions which return varying // numbers of residuals at runtime. AutoDiffCostFunction(CostFunctor* functor, int num_residuals) : functor_(functor) { - CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the " - << "number of residuals is not ceres::DYNAMIC."; - SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> + CHECK_EQ(kNumResiduals, DYNAMIC) + << "Can't run the dynamic-size constructor if the " + << "number of residuals is not ceres::DYNAMIC."; + SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> ::set_num_residuals(num_residuals); } @@ -206,7 +209,7 @@ N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( *functor_, parameters, - SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> + SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> ::num_residuals(), residuals, jacobians);
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h index 4058366..5048348 100644 --- a/include/ceres/internal/numeric_diff.h +++ b/include/ceres/internal/numeric_diff.h
@@ -90,6 +90,7 @@ const CostFunctor* functor, double const* residuals_at_eval_point, const double relative_step_size, + int num_residuals, double **parameters, double *jacobian) { using Eigen::Map; @@ -97,15 +98,21 @@ using Eigen::RowMajor; using Eigen::ColMajor; + const int NUM_RESIDUALS = + (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals); + typedef Matrix<double, kNumResiduals, 1> ResidualVector; typedef Matrix<double, kParameterBlockSize, 1> ParameterVector; - typedef Matrix<double, kNumResiduals, kParameterBlockSize, + typedef Matrix<double, + kNumResiduals, + kParameterBlockSize, (kParameterBlockSize == 1 && - kNumResiduals > 1) ? ColMajor : RowMajor> JacobianMatrix; + kNumResiduals > 1) ? ColMajor : RowMajor> + JacobianMatrix; Map<JacobianMatrix> parameter_jacobian(jacobian, - kNumResiduals, + NUM_RESIDUALS, kParameterBlockSize); // Mutate 1 element at a time and then restore. @@ -125,16 +132,16 @@ // For each parameter in the parameter block, use finite differences to // compute the derivative for that parameter. + + ResidualVector residuals(NUM_RESIDUALS); 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<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor, parameters, residuals, functor)) { + functor, parameters, residuals.data(), functor)) { return false; } @@ -142,8 +149,7 @@ // 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, kNumResiduals); + parameter_jacobian.col(j) = residuals; double one_over_delta = 1.0 / delta; if (kMethod == CENTRAL) { @@ -151,17 +157,16 @@ x_plus_delta(j) = x(j) - delta; if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor, parameters, residuals, functor)) { + functor, parameters, residuals.data(), functor)) { return false; } - parameter_jacobian.col(j) -= - Map<ResidualVector>(residuals, kNumResiduals, 1); + parameter_jacobian.col(j) -= residuals; one_over_delta /= 2; } else { // Forward difference only; reuse existing residuals evaluation. parameter_jacobian.col(j) -= - Map<const ResidualVector>(residuals_at_eval_point, kNumResiduals); + Map<const ResidualVector>(residuals_at_eval_point, NUM_RESIDUALS); } x_plus_delta(j) = x(j); // Restore x_plus_delta. @@ -186,6 +191,7 @@ const CostFunctor* functor, double const* residuals_at_eval_point, const double relative_step_size, + const int num_residuals, double **parameters, double *jacobian) { LOG(FATAL) << "Control should never reach here.";
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h index a47a66d..94573e5 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h
@@ -95,6 +95,21 @@ // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing // a 1-dimensional output from two arguments, both 2-dimensional. // +// NumericDiffCostFunction also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// CostFunction* cost_function +// = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>( +// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ +// TAKE_OWNERSHIP, | | | +// 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 10 // independent variables, and there is no limit on the dimensionality // of each of them. @@ -104,8 +119,6 @@ // 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 @@ -177,17 +190,17 @@ N5, N6, N7, N8, N9> { public: NumericDiffCostFunction(CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + int num_residuals = kNumResiduals, 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) {} + ownership_(ownership), + relative_step_size_(relative_step_size) { + if (kNumResiduals == DYNAMIC) { + SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> + ::set_num_residuals(num_residuals); + } + } ~NumericDiffCostFunction() { if (ownership_ != TAKE_OWNERSHIP) { @@ -216,7 +229,7 @@ return false; } - if (!jacobians) { + if (jacobians == NULL) { return true; } @@ -264,6 +277,7 @@ functor_.get(), \ residuals, \ relative_step_size_, \ + SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::num_residuals(), \ parameters_reference_copy.get(), \ jacobians[block])) { \ return false; \
diff --git a/internal/ceres/numeric_diff_cost_function_test.cc b/internal/ceres/numeric_diff_cost_function_test.cc index 3953ded..422c712 100644 --- a/internal/ceres/numeric_diff_cost_function_test.cc +++ b/internal/ceres/numeric_diff_cost_function_test.cc
@@ -184,5 +184,18 @@ new SizeTestingCostFunction<2,2>, ceres::TAKE_OWNERSHIP)); } +TEST(NumericDiffCostFunction, EasyCaseFunctorCentralDifferencesAndDynamicNumResiduals) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( + new NumericDiffCostFunction<EasyFunctor, + CENTRAL, + ceres::DYNAMIC, + 5, /* size of x1 */ + 5 /* size of x2 */>( + new EasyFunctor, TAKE_OWNERSHIP, 3)); + EasyFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} + } // namespace internal } // namespace ceres