From 4cd257cf4ac8b0b81ab76b5dbac53b2b01de885e Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Thu, 15 Sep 2022 15:10:13 -0700 Subject: [PATCH] Let NumericDiffFirstOrderFunction take a dynamically sized parameter vector Also fix a template naming lint along the way. Change-Id: Iabb98aeec2ff9609a19c3778b9ea2da37771c985 --- .../dynamic_numeric_diff_cost_function.h | 4 +- include/ceres/internal/variadic_evaluate.h | 23 +++++ include/ceres/numeric_diff_cost_function.h | 4 +- .../ceres/numeric_diff_first_order_function.h | 95 +++++++++++++++---- .../numeric_diff_first_order_function_test.cc | 32 ++++++- 5 files changed, 129 insertions(+), 29 deletions(-) diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h index e1892e8ba..4b3e5433d 100644 --- a/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/include/ceres/dynamic_numeric_diff_cost_function.h @@ -76,7 +76,7 @@ namespace ceres { // cost_function.AddParameterBlock(5); // cost_function.AddParameterBlock(10); // cost_function.SetNumResiduals(21); -template +template class DynamicNumericDiffCostFunction final : public DynamicCostFunction { public: explicit DynamicNumericDiffCostFunction( @@ -134,7 +134,7 @@ class DynamicNumericDiffCostFunction final : public DynamicCostFunction { for (size_t block = 0; block < block_sizes.size(); ++block) { if (jacobians[block] != nullptr && !NumericDiff(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 +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 6ec531750..cbdbfdaef 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h @@ -176,7 +176,7 @@ namespace ceres { template // Parameters dimensions for each block. class NumericDiffCostFunction final @@ -236,7 +236,7 @@ class NumericDiffCostFunction final } internal::EvaluateJacobianForParameterBlocks:: - template Apply( + template Apply( 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 f5bb005be..0dc6d61b3 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 @@ namespace ceres { // "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( +// 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 + 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 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction { bool Evaluate(const double* const parameters, double* cost, double* gradient) const override { - using ParameterDims = internal::StaticParameterDims; - constexpr int kNumResiduals = 1; - // Get the function value (cost) at the the point to evaluate. - if (!internal::VariadicEvaluate( - *functor_, ¶meters, cost)) { + if (!(*functor_)(parameters, cost)) { return false; } @@ -135,27 +168,47 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction { } // Create a copy of the parameters which will get mutated. - internal::FixedArray parameters_copy(kNumParameters); - std::copy_n(parameters, kNumParameters, parameters_copy.data()); + internal::FixedArray parameters_copy(num_parameters_); + std::copy_n(parameters, num_parameters_, parameters_copy.data()); double* parameters_ptr = parameters_copy.data(); - internal::EvaluateJacobianForParameterBlocks< - ParameterDims>::template Apply(functor_.get(), - cost, - options_, - kNumResiduals, - ¶meters_ptr, - &gradient); - return true; + constexpr int kNumResiduals = 1; + if constexpr (kNumParameters == DYNAMIC) { + internal::FirstOrderFunctorAdapter fofa(*functor_); + return internal::NumericDiff< + internal::FirstOrderFunctorAdapter, + kMethod, + kNumResiduals, + internal::DynamicParameterDims, + 0, + DYNAMIC>::EvaluateJacobianForParameterBlock(&fofa, + cost, + options_, + kNumResiduals, + 0, + num_parameters_, + ¶meters_ptr, + gradient); + } else { + return internal::EvaluateJacobianForParameterBlocks< + internal::StaticParameterDims>:: + template Apply(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 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 25c58f3e5..d6fab4879 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 @@ class QuadraticCostFunctor { double a_; }; -TEST(NumericDiffFirstOrderFunction, BilinearDifferentiationTest) { - std::unique_ptr function( - new NumericDiffFirstOrderFunction( - new QuadraticCostFunctor(1.0))); +TEST(NumericDiffFirstOrderFunction, BilinearDifferentiationTestStatic) { + auto function = std::make_unique< + NumericDiffFirstOrderFunction>( + 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>( + new QuadraticCostFunctor(1.0), 4); double parameters[4] = {1.0, 2.0, 3.0, 4.0}; double gradient[4];