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];
