Let NumericDiffFirstOrderFunction take a dynamically sized parameter vector Also fix a template naming lint along the way. Change-Id: Iabb98aeec2ff9609a19c3778b9ea2da37771c985
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h index e1892e8..4b3e543 100644 --- a/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -76,7 +76,7 @@ // cost_function.AddParameterBlock(5); // cost_function.AddParameterBlock(10); // cost_function.SetNumResiduals(21); -template <typename CostFunctor, NumericDiffMethodType method = CENTRAL> +template <typename CostFunctor, NumericDiffMethodType kMethod = CENTRAL> class DynamicNumericDiffCostFunction final : public DynamicCostFunction { public: explicit DynamicNumericDiffCostFunction( @@ -134,7 +134,7 @@ for (size_t block = 0; block < block_sizes.size(); ++block) { if (jacobians[block] != nullptr && !NumericDiff<CostFunctor, - method, + kMethod, ceres::DYNAMIC, internal::DynamicParameterDims, ceres::DYNAMIC,
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h index dcd5227..e9c0a83 100644 --- a/include/ceres/internal/variadic_evaluate.h +++ b/include/ceres/internal/variadic_evaluate.h
@@ -106,6 +106,29 @@ return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor); } +// When differentiating dynamically sized CostFunctions, VariadicEvaluate +// expects a functor with the signature: +// +// bool operator()(double const* const* parameters, double* cost) const +// +// However for NumericDiffFirstOrderFunction, the functor has the signature +// +// bool operator()(double const* parameters, double* cost) const +// +// This thin wrapper adapts the latter to the former. +template <typename Functor> +class FirstOrderFunctorAdapter { + public: + explicit FirstOrderFunctorAdapter(const Functor& functor) + : functor_(functor) {} + bool operator()(double const* const* parameters, double* cost) const { + return functor_(*parameters, cost); + } + + private: + const Functor& functor_; +}; + } // namespace ceres::internal #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 6ec5317..cbdbfda 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h
@@ -176,7 +176,7 @@ namespace ceres { template <typename CostFunctor, - NumericDiffMethodType method = CENTRAL, + NumericDiffMethodType kMethod = CENTRAL, int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC int... Ns> // Parameters dimensions for each block. class NumericDiffCostFunction final @@ -236,7 +236,7 @@ } internal::EvaluateJacobianForParameterBlocks<ParameterDims>:: - template Apply<method, kNumResiduals>( + template Apply<kMethod, kNumResiduals>( functor_.get(), residuals, options_,
diff --git a/include/ceres/numeric_diff_first_order_function.h b/include/ceres/numeric_diff_first_order_function.h index f5bb005..0dc6d61 100644 --- a/include/ceres/numeric_diff_first_order_function.h +++ b/include/ceres/numeric_diff_first_order_function.h
@@ -42,6 +42,7 @@ #include "ceres/internal/variadic_evaluate.h" #include "ceres/numeric_diff_options.h" #include "ceres/types.h" +#include "glog/logging.h" namespace ceres { @@ -99,19 +100,55 @@ // "QuadraticCostFunctor", "CENTRAL, 4", describe the finite // differencing scheme as "central differencing" and the functor as // computing its cost from a 4 dimensional input. +// +// If the size of the parameter vector is not known at compile time, then an +// alternate construction syntax can be used: +// +// FirstOrderFunction* function +// = new NumericDiffFirstOrderFunction<MyScalarCostFunctor, CENTRAL>( +// new QuadraticCostFunctor(1.0), 4); +// +// Note that instead of passing 4 as a template argument, it is now passed as +// the second argument to the constructor. template <typename FirstOrderFunctor, - NumericDiffMethodType method, - int kNumParameters> + NumericDiffMethodType kMethod, + int kNumParameters = DYNAMIC> class NumericDiffFirstOrderFunction final : public FirstOrderFunction { public: + // Constructor for the case where the parameter size is known at compile time. explicit NumericDiffFirstOrderFunction( FirstOrderFunctor* functor, Ownership ownership = TAKE_OWNERSHIP, const NumericDiffOptions& options = NumericDiffOptions()) - : functor_(functor), ownership_(ownership), options_(options) { + : functor_(functor), + num_parameters_(kNumParameters), + ownership_(ownership), + options_(options) { + static_assert(kNumParameters != DYNAMIC, + "Number of parameters must be static when defined via the " + "template parameter. Use the other constructor for " + "dynamically sized functions."); static_assert(kNumParameters > 0, "kNumParameters must be positive"); } + // Constructor for the case where the parameter size is specified at run time. + explicit NumericDiffFirstOrderFunction( + FirstOrderFunctor* functor, + int num_parameters, + Ownership ownership = TAKE_OWNERSHIP, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), + num_parameters_(num_parameters), + ownership_(ownership), + options_(options) { + static_assert( + kNumParameters == DYNAMIC, + "Template parameter must be DYNAMIC when using this constructor. If " + "you want to provide the number of parameters statically use the other " + "constructor."); + CHECK_GT(num_parameters, 0); + } + ~NumericDiffFirstOrderFunction() override { if (ownership_ != TAKE_OWNERSHIP) { functor_.release(); @@ -121,12 +158,8 @@ bool Evaluate(const double* const parameters, double* cost, double* gradient) const override { - using ParameterDims = internal::StaticParameterDims<kNumParameters>; - constexpr int kNumResiduals = 1; - // Get the function value (cost) at the the point to evaluate. - if (!internal::VariadicEvaluate<ParameterDims>( - *functor_, ¶meters, cost)) { + if (!(*functor_)(parameters, cost)) { return false; } @@ -135,27 +168,47 @@ } // Create a copy of the parameters which will get mutated. - internal::FixedArray<double, 32> parameters_copy(kNumParameters); - std::copy_n(parameters, kNumParameters, parameters_copy.data()); + internal::FixedArray<double, 32> parameters_copy(num_parameters_); + std::copy_n(parameters, num_parameters_, parameters_copy.data()); double* parameters_ptr = parameters_copy.data(); - internal::EvaluateJacobianForParameterBlocks< - ParameterDims>::template Apply<method, kNumResiduals>(functor_.get(), - cost, - options_, - kNumResiduals, - ¶meters_ptr, - &gradient); - return true; + constexpr int kNumResiduals = 1; + if constexpr (kNumParameters == DYNAMIC) { + internal::FirstOrderFunctorAdapter<FirstOrderFunctor> fofa(*functor_); + return internal::NumericDiff< + internal::FirstOrderFunctorAdapter<FirstOrderFunctor>, + kMethod, + kNumResiduals, + internal::DynamicParameterDims, + 0, + DYNAMIC>::EvaluateJacobianForParameterBlock(&fofa, + cost, + options_, + kNumResiduals, + 0, + num_parameters_, + ¶meters_ptr, + gradient); + } else { + return internal::EvaluateJacobianForParameterBlocks< + internal::StaticParameterDims<kNumParameters>>:: + template Apply<kMethod, 1>(functor_.get(), + cost, + options_, + kNumResiduals, + ¶meters_ptr, + &gradient); + } } - int NumParameters() const override { return kNumParameters; } + int NumParameters() const override { return num_parameters_; } const FirstOrderFunctor& functor() const { return *functor_; } private: std::unique_ptr<FirstOrderFunctor> functor_; - Ownership ownership_; - NumericDiffOptions options_; + const int num_parameters_; + const Ownership ownership_; + const NumericDiffOptions options_; }; } // namespace ceres
diff --git a/internal/ceres/numeric_diff_first_order_function_test.cc b/internal/ceres/numeric_diff_first_order_function_test.cc index 25c58f3..d6fab48 100644 --- a/internal/ceres/numeric_diff_first_order_function_test.cc +++ b/internal/ceres/numeric_diff_first_order_function_test.cc
@@ -50,10 +50,34 @@ double a_; }; -TEST(NumericDiffFirstOrderFunction, BilinearDifferentiationTest) { - std::unique_ptr<FirstOrderFunction> function( - new NumericDiffFirstOrderFunction<QuadraticCostFunctor, CENTRAL, 4>( - new QuadraticCostFunctor(1.0))); +TEST(NumericDiffFirstOrderFunction, BilinearDifferentiationTestStatic) { + auto function = std::make_unique< + NumericDiffFirstOrderFunction<QuadraticCostFunctor, CENTRAL, 4>>( + new QuadraticCostFunctor(1.0)); + + double parameters[4] = {1.0, 2.0, 3.0, 4.0}; + double gradient[4]; + double cost; + + function->Evaluate(parameters, &cost, nullptr); + EXPECT_EQ(cost, 13.0); + + cost = -1.0; + function->Evaluate(parameters, &cost, gradient); + + EXPECT_EQ(cost, 13.0); + + const double kTolerance = 1e-9; + EXPECT_NEAR(gradient[0], parameters[1], kTolerance); + EXPECT_NEAR(gradient[1], parameters[0], kTolerance); + EXPECT_NEAR(gradient[2], parameters[3], kTolerance); + EXPECT_NEAR(gradient[3], parameters[2], kTolerance); +} + +TEST(NumericDiffFirstOrderFunction, BilinearDifferentiationTestDynamic) { + auto function = std::make_unique< + NumericDiffFirstOrderFunction<QuadraticCostFunctor, CENTRAL>>( + new QuadraticCostFunctor(1.0), 4); double parameters[4] = {1.0, 2.0, 3.0, 4.0}; double gradient[4];