Benchmarks for dynamic autodiff. This patch is from Clement Courbet. courbet@google.com Change-Id: I886390663644733bfa5b7b52b0c883079e793726
diff --git a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc b/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc index f8b2a21..2533ba6 100644 --- a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc +++ b/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc
@@ -30,6 +30,7 @@ #include <memory> #include <random> +#include <utility> #include "benchmark/benchmark.h" #include "ceres/autodiff_benchmarks/brdf_cost_function.h" @@ -41,31 +42,33 @@ #include "ceres/ceres.h" namespace ceres { -namespace internal { -// If we want to use functors with both operator() and an Evaluate() method -// with AutoDiff then this wrapper class here has to be used. Autodiff doesn't -// support functors that have an Evaluate() function. -// -// CostFunctionToFunctor hides the Evaluate() function, because it doesn't -// derive from CostFunction. Autodiff sees it as a simple functor and will use -// the operator() as expected. -template <typename CostFunction> -struct CostFunctionToFunctor { - template <typename... _Args> - explicit CostFunctionToFunctor(_Args&&... __args) - : cost_function(std::forward<_Args>(__args)...) {} +enum Dynamic { kNotDynamic, kDynamic }; - template <typename... _Args> - inline bool operator()(_Args&&... __args) const { - return cost_function(std::forward<_Args>(__args)...); - } +// Transforms a static functor into a dynamic one. +template <typename CostFunctionType, int kNumParameterBlocks> +class ToDynamic { + public: + template <typename... _Args> + explicit ToDynamic(_Args&&... __args) + : cost_function_(std::forward<_Args>(__args)...) {} - CostFunction cost_function; + template <typename T> + bool operator()(const T* const* parameters, T* residuals) const { + return Apply(parameters, residuals, + std::make_index_sequence<kNumParameterBlocks>()); + } + + private: + template <typename T, size_t... Indices> + bool Apply(const T* const* parameters, T* residuals, + std::index_sequence<Indices...>) const { + return cost_function_(parameters[Indices]..., residuals); + } + + CostFunctionType cost_function_; }; -} // namespace internal - template <int kParameterBlockSize> static void BM_ConstantAnalytic(benchmark::State& state) { constexpr int num_residuals = 1; @@ -79,14 +82,59 @@ double* jacobians[] = {jacobian_values.data()}; std::unique_ptr<ceres::CostFunction> cost_function( - new ConstantCostFunction<kParameterBlockSize>()); + new AnalyticConstantCostFunction<kParameterBlockSize>()); for (auto _ : state) { cost_function->Evaluate(parameters, residuals.data(), jacobians); } } -template <int kParameterBlockSize> +// Helpers for CostFunctionFactory. +template <typename DynamicCostFunctionType> +void AddParameterBlocks(DynamicCostFunctionType*) {} + +template <int HeadN, int... TailNs, typename DynamicCostFunctionType> +void AddParameterBlocks(DynamicCostFunctionType* dynamic_function) { + dynamic_function->AddParameterBlock(HeadN); + AddParameterBlocks<TailNs...>(dynamic_function); +} + +// Creates an autodiff cost function wrapping `CostFunctor`, with +// `kNumResiduals` residuals and parameter blocks with sized `Ns..`. +// Depending on `kIsDynamic`, either a static or dynamic cost function is +// created. +// `args` are forwarded to the `CostFunctor` constructor. +template <Dynamic kIsDynamic> +struct CostFunctionFactory {}; + +template <> +struct CostFunctionFactory<kNotDynamic> { + template <typename CostFunctor, int kNumResiduals, int... Ns, + typename... Args> + static std::unique_ptr<ceres::CostFunction> Create(Args&&... args) { + return std::make_unique< + ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns...>>( + new CostFunctor(std::forward<Args>(args)...)); + } +}; + +template <> +struct CostFunctionFactory<kDynamic> { + template <typename CostFunctor, int kNumResiduals, int... Ns, + typename... Args> + static std::unique_ptr<ceres::CostFunction> Create(Args&&... args) { + constexpr const int kNumParameterBlocks = sizeof...(Ns); + auto dynamic_function = std::make_unique<ceres::DynamicAutoDiffCostFunction< + ToDynamic<CostFunctor, kNumParameterBlocks>>>( + new ToDynamic<CostFunctor, kNumParameterBlocks>( + std::forward<Args>(args)...)); + dynamic_function->SetNumResiduals(kNumResiduals); + AddParameterBlocks<Ns...>(dynamic_function.get()); + return dynamic_function; + } +}; + +template <int kParameterBlockSize, Dynamic kIsDynamic> static void BM_ConstantAutodiff(benchmark::State& state) { constexpr int num_residuals = 1; std::array<double, kParameterBlockSize> parameters_values; @@ -98,11 +146,9 @@ std::array<double, num_residuals * kParameterBlockSize> jacobian_values; double* jacobians[] = {jacobian_values.data()}; - using AutoDiffFunctor = ceres::internal::CostFunctionToFunctor< - ConstantCostFunction<kParameterBlockSize>>; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<AutoDiffFunctor, 1, kParameterBlockSize>( - new AutoDiffFunctor())); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create< + ConstantCostFunction<kParameterBlockSize>, 1, 1>(); for (auto _ : state) { cost_function->Evaluate(parameters, residuals.data(), jacobians); @@ -110,24 +156,29 @@ } BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 1); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 1); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 1, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 1, kDynamic); BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 10); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 10); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 10, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 10, kDynamic); BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 20); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 20); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 20, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 20, kDynamic); BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 30); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 30); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 30, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 30, kDynamic); BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 40); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 40); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 40, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 40, kDynamic); BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 50); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 50); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 50, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 50, kDynamic); BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 60); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 60); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 60, kNotDynamic); +BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 60, kDynamic); +template <Dynamic kIsDynamic> static void BM_Linear1AutoDiff(benchmark::State& state) { - using FunctorType = - ceres::internal::CostFunctionToFunctor<Linear1CostFunction>; - double parameter_block1[] = {1.}; double* parameters[] = {parameter_block1}; @@ -135,20 +186,20 @@ double residuals[1]; double* jacobians[] = {jacobian1}; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<FunctorType, 1, 1>(new FunctorType())); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create<Linear1CostFunction, 1, + 1>(); for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_Linear1AutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_Linear1AutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_Linear1AutoDiff, kDynamic)->Arg(0)->Arg(1); +template <Dynamic kIsDynamic> static void BM_Linear10AutoDiff(benchmark::State& state) { - using FunctorType = - ceres::internal::CostFunctionToFunctor<Linear10CostFunction>; - double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7., 8., 9., 10.}; double* parameters[] = {parameter_block1}; @@ -156,15 +207,17 @@ double residuals[10]; double* jacobians[] = {jacobian1}; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<FunctorType, 10, 10>(new FunctorType())); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create<Linear10CostFunction, 10, + 10>(); for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_Linear10AutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_Linear10AutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_Linear10AutoDiff, kDynamic)->Arg(0)->Arg(1); // From the NIST problem collection. struct Rat43CostFunctor { @@ -180,11 +233,14 @@ return true; } + static constexpr int kNumParameterBlocks = 1; + private: const double x_; const double y_; }; +template <Dynamic kIsDynamic> static void BM_Rat43AutoDiff(benchmark::State& state) { double parameter_block1[] = {1., 2., 3., 4.}; double* parameters[] = {parameter_block1}; @@ -194,21 +250,20 @@ double* jacobians[] = {jacobian1}; const double x = 0.2; const double y = 0.3; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<Rat43CostFunctor, 1, 4>( - new Rat43CostFunctor(x, y))); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create<Rat43CostFunctor, 1, 4>( + x, y); for (auto _ : state) { - cost_function->Evaluate( - parameters, &residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, &residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_Rat43AutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_Rat43AutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_Rat43AutoDiff, kDynamic)->Arg(0)->Arg(1); +template <Dynamic kIsDynamic> static void BM_SnavelyReprojectionAutoDiff(benchmark::State& state) { - using FunctorType = - ceres::internal::CostFunctionToFunctor<SnavelyReprojectionError>; - double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7., 8., 9.}; double parameter_block2[] = {1., 2., 3.}; double* parameters[] = {parameter_block1, parameter_block2}; @@ -220,18 +275,20 @@ const double x = 0.2; const double y = 0.3; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<FunctorType, 2, 9, 3>( - new FunctorType(x, y))); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create<SnavelyReprojectionError, + 2, 9, 3>(x, y); for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_SnavelyReprojectionAutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_SnavelyReprojectionAutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_SnavelyReprojectionAutoDiff, kDynamic)->Arg(0)->Arg(1); +template <Dynamic kIsDynamic> static void BM_PhotometricAutoDiff(benchmark::State& state) { constexpr int PATCH_SIZE = 8; @@ -280,22 +337,22 @@ FunctorType::Intrinsics intrinsics; intrinsics << 128, 128, 1, -1, 0.5, 0.5; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<FunctorType, - FunctorType::PATCH_SIZE, - FunctorType::POSE_SIZE, - FunctorType::POSE_SIZE, - FunctorType::POINT_SIZE>(new FunctorType( - intensities_host, bearings_host, image_target, intrinsics))); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create< + FunctorType, FunctorType::PATCH_SIZE, FunctorType::POSE_SIZE, + FunctorType::POSE_SIZE, FunctorType::POINT_SIZE>( + intensities_host, bearings_host, image_target, intrinsics); for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_PhotometricAutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_PhotometricAutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_PhotometricAutoDiff, kDynamic)->Arg(0)->Arg(1); +template <Dynamic kIsDynamic> static void BM_RelativePoseAutoDiff(benchmark::State& state) { using FunctorType = RelativePoseError; @@ -314,20 +371,22 @@ Eigen::Quaterniond q_i_j = Eigen::Quaterniond(1, 2, 3, 4).normalized(); Eigen::Vector3d t_i_j(1, 2, 3); - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<FunctorType, 6, 7, 7>( - new FunctorType(q_i_j, t_i_j))); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create<FunctorType, 6, 7, 7>( + q_i_j, t_i_j); for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_RelativePoseAutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_RelativePoseAutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_RelativePoseAutoDiff, kDynamic)->Arg(0)->Arg(1); +template <Dynamic kIsDynamic> static void BM_BrdfAutoDiff(benchmark::State& state) { - using FunctorType = ceres::internal::CostFunctionToFunctor<Brdf>; + using FunctorType = Brdf; double material[] = {1., 2., 3., 4., 5., 6., 7., 8., 9., 10.}; auto c = Eigen::Vector3d(0.1, 0.2, 0.3); @@ -337,32 +396,29 @@ auto x = Eigen::Vector3d(0.5, 0.7, -0.1).normalized(); auto y = Eigen::Vector3d(0.2, -0.2, -0.2).normalized(); - double* parameters[7] = { - material, c.data(), n.data(), v.data(), l.data(), x.data(), y.data()}; + double* parameters[7] = {material, c.data(), n.data(), v.data(), + l.data(), x.data(), y.data()}; double jacobian[(10 + 6 * 3) * 3]; double residuals[3]; double* jacobians[7] = { - jacobian + 0, - jacobian + 10 * 3, - jacobian + 13 * 3, - jacobian + 16 * 3, - jacobian + 19 * 3, - jacobian + 22 * 3, + jacobian + 0, jacobian + 10 * 3, jacobian + 13 * 3, + jacobian + 16 * 3, jacobian + 19 * 3, jacobian + 22 * 3, jacobian + 25 * 3, }; - std::unique_ptr<ceres::CostFunction> cost_function( - new ceres::AutoDiffCostFunction<FunctorType, 3, 10, 3, 3, 3, 3, 3, 3>( - new FunctorType)); + std::unique_ptr<ceres::CostFunction> cost_function = + CostFunctionFactory<kIsDynamic>::template Create<FunctorType, 3, 10, 3, 3, + 3, 3, 3, 3>(); for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); + cost_function->Evaluate(parameters, residuals, + state.range(0) ? jacobians : nullptr); } } -BENCHMARK(BM_BrdfAutoDiff)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_BrdfAutoDiff, kNotDynamic)->Arg(0)->Arg(1); +BENCHMARK_TEMPLATE(BM_BrdfAutoDiff, kDynamic)->Arg(0)->Arg(1); } // namespace ceres
diff --git a/internal/ceres/autodiff_benchmarks/constant_cost_function.h b/internal/ceres/autodiff_benchmarks/constant_cost_function.h index caa0431..2bbe4cb 100644 --- a/internal/ceres/autodiff_benchmarks/constant_cost_function.h +++ b/internal/ceres/autodiff_benchmarks/constant_cost_function.h
@@ -37,14 +37,17 @@ namespace ceres { template <int kParameterBlockSize> -struct ConstantCostFunction - : public ceres::SizedCostFunction<1, kParameterBlockSize> { +struct ConstantCostFunction { template <typename T> inline bool operator()(const T* const x, T* residuals) const { residuals[0] = T(5); return true; } +}; +template <int kParameterBlockSize> +struct AnalyticConstantCostFunction + : public ceres::SizedCostFunction<1, kParameterBlockSize> { virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {