Sized cost function using variadic templates This PR changes the interface of sized_cost_fucntion, autodiff_cost_function and numeric_diff_costfunction from using ten hardcoded parameter blocks to a variable number of parameter blocks using variadic templates. Trailing parameter blocks of size zero are now considered as error. Change-Id: I37b9a0a420ef0eda6476a46672bbf6bd57e19760
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h index 23ed456..4a52cbc 100644 --- a/include/ceres/autodiff_cost_function.h +++ b/include/ceres/autodiff_cost_function.h
@@ -110,10 +110,6 @@ // 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. -// // WARNING #1: Since the functor will get instantiated with different types for // T, you must convert from other numeric types to T before mixing // computations with other variables of type T. In the example above, this is @@ -153,19 +149,8 @@ // of residuals for a single autodiff cost function at runtime. template <typename CostFunctor, 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 AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> { + int... Ns> // Number of parameters in each parameter block. +class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { public: // Takes ownership of functor. Uses the template-provided value for the // number of residuals ("kNumResiduals"). @@ -186,10 +171,7 @@ 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); + SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals); } virtual ~AutoDiffCostFunction() {} @@ -202,20 +184,20 @@ virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { + using ParameterDims = + typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims; + if (!jacobians) { - return internal::VariadicEvaluate< - CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> - ::Call(*functor_, parameters, residuals); + return internal::VariadicEvaluate<ParameterDims>(*functor_, + parameters, + residuals); } - return internal::AutoDiff<CostFunctor, double, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( - *functor_, - parameters, - SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9>::num_residuals(), - residuals, - jacobians); + return internal::AutoDifferentiate<ParameterDims>( + *functor_, + parameters, + SizedCostFunction<kNumResiduals, Ns...>::num_residuals(), + residuals, + jacobians); } private:
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h index 257e937..649e05d 100644 --- a/include/ceres/autodiff_local_parameterization.h +++ b/include/ceres/autodiff_local_parameterization.h
@@ -134,12 +134,9 @@ const double* parameter_ptrs[2] = {x, zero_delta}; double* jacobian_ptrs[2] = { NULL, jacobian }; - return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize> - ::Differentiate(*functor_, - parameter_ptrs, - kGlobalSize, - x_plus_delta, - jacobian_ptrs); + return internal::AutoDifferentiate< + internal::StaticParameterDims<kGlobalSize, kLocalSize>>( + *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); } virtual int GlobalSize() const { return kGlobalSize; }
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h index d5806ee..33ac5e1 100644 --- a/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -42,6 +42,7 @@ #include "ceres/dynamic_cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/internal/numeric_diff.h" +#include "ceres/internal/parameter_dims.h" #include "ceres/numeric_diff_options.h" #include "glog/logging.h" @@ -103,7 +104,11 @@ << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " << "before DynamicNumericDiffCostFunction::Evaluate()."; - const bool status = EvaluateCostFunctor(parameters, residuals); + const bool status = + internal::VariadicEvaluate<internal::DynamicParameterDims>( + *functor_.get(), + parameters, + residuals); if (jacobians == NULL || !status) { return status; } @@ -127,18 +132,18 @@ for (size_t block = 0; block < block_sizes.size(); ++block) { if (jacobians[block] != NULL && - !NumericDiff<CostFunctor, method, DYNAMIC, - DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, - DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, - DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock( - functor_.get(), - residuals, - options_, - this->num_residuals(), - block, - block_sizes[block], - ¶meters_references_copy[0], - jacobians[block])) { + !NumericDiff<CostFunctor, method, ceres::DYNAMIC, + internal::DynamicParameterDims, ceres::DYNAMIC, + ceres::DYNAMIC>:: + EvaluateJacobianForParameterBlock( + functor_.get(), + residuals, + options_, + this->num_residuals(), + block, + block_sizes[block], + ¶meters_references_copy[0], + jacobians[block])) { return false; } } @@ -146,30 +151,6 @@ } private: - bool EvaluateCostFunctor(double const* const* parameters, - double* residuals) const { - return EvaluateCostFunctorImpl(functor_.get(), - parameters, - residuals, - functor_.get()); - } - - // Helper templates to allow evaluation of a functor or a - // CostFunction. - bool EvaluateCostFunctorImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const void* /* NOT USED */) const { - return (*functor)(parameters, residuals); - } - - bool EvaluateCostFunctorImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const CostFunction* /* NOT USED */) const { - return functor->Evaluate(parameters, residuals, NULL); - } - std::unique_ptr<const CostFunctor> functor_; Ownership ownership_; NumericDiffOptions options_;
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index 903c89b..ff47fbf 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h
@@ -33,7 +33,7 @@ // dual numbers in jet.h. Before reading the rest of this file, it is advisable // to read jet.h's header comment in detail. // -// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// The helper wrapper AutoDifferentiate() computes the jacobian of // functors with templated operator() taking this form: // // struct F { @@ -142,10 +142,14 @@ #include <stddef.h> -#include "ceres/jet.h" +#include <array> + #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" +#include "ceres/internal/parameter_dims.h" #include "ceres/internal/variadic_evaluate.h" +#include "ceres/jet.h" +#include "ceres/types.h" #include "glog/logging.h" namespace ceres { @@ -165,21 +169,51 @@ // // is what would get put in dst if N was 3, offset was 3, and the jet type JetT // was 8-dimensional. -template <typename JetT, typename T, int N> -inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { +template <int Offset, int N, typename T, typename JetT> +inline void Make1stOrderPerturbation(const T* src, JetT* dst) { DCHECK(src); DCHECK(dst); for (int j = 0; j < N; ++j) { dst[j].a = src[j]; dst[j].v.setZero(); - dst[j].v[offset + j] = T(1.0); + dst[j].v[Offset + j] = T(1.0); } } +// Calls Make1stOrderPerturbation for every parameter block. +// +// Example: +// If one having three parameter blocks with dimensions (3, 2, 4), the call +// Make1stOrderPerturbations<integer_sequence<3, 2, 4>::Apply(params, x); +// will result in the following calls to Make1stOrderPerturbation: +// Make1stOrderPerturbation<0, 3>(params[0], x + 0); +// Make1stOrderPerturbation<3, 2>(params[1], x + 3); +// Make1stOrderPerturbation<5, 4>(params[2], x + 5); +template <typename Seq, int ParameterIdx = 0, int Offset = 0> +struct Make1stOrderPerturbations; + +template <int N, int... Ns, int ParameterIdx, int Offset> +struct Make1stOrderPerturbations<integer_sequence<int, N, Ns...>, ParameterIdx, + Offset> { + template <typename T, typename JetT> + static void Apply(T const* const* parameters, JetT* x) { + Make1stOrderPerturbation<Offset, N>(parameters[ParameterIdx], x + Offset); + Make1stOrderPerturbations<integer_sequence<int, Ns...>, ParameterIdx + 1, + Offset + N>::Apply(parameters, x); + } +}; + +// End of 'recursion'. Nothing more to do. +template <int ParameterIdx, int Total> +struct Make1stOrderPerturbations<integer_sequence<int>, ParameterIdx, Total> { + template <typename T, typename JetT> + static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {} +}; + // Takes the 0th order part of src, assumed to be a Jet type, and puts it in // dst. This is used to pick out the "vector" part of the extended y. template <typename JetT, typename T> -inline void Take0thOrderPart(int M, const JetT *src, T dst) { +inline void Take0thOrderPart(int M, const JetT* src, T dst) { DCHECK(src); for (int i = 0; i < M; ++i) { dst[i] = src[i].a; @@ -188,8 +222,8 @@ // Takes N 1st order parts, starting at index N0, and puts them in the M x N // matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. -template <typename JetT, typename T, int N0, int N> -inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { +template <int N0, int N, typename JetT, typename T> +inline void Take1stOrderPart(const int M, const JetT* src, T* dst) { DCHECK(src); DCHECK(dst); for (int i = 0; i < M; ++i) { @@ -198,126 +232,86 @@ } } -// This is in a struct because default template parameters on a -// function are not supported in C++03 (though it is available in -// C++0x). N0 through N9 are the dimension of the input arguments to -// the user supplied functor. -template <typename Functor, typename T, - 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> -struct AutoDiff { - static bool Differentiate(const Functor& functor, - T const *const *parameters, - int num_outputs, - T *function_value, - T **jacobians) { - // This block breaks the 80 column rule to keep it somewhat readable. - DCHECK_GT(num_outputs, 0); - DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT - << "Zero block cannot precede a non-zero block. Block sizes are " - << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " - << N8 << ", " << N9; +// Calls Take1stOrderPart for every parameter block. +// +// Example: +// If one having three parameter blocks with dimensions (3, 2, 4), the call +// Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs, +// output, +// jacobians); +// will result in the following calls to Take1stOrderPart: +// if (jacobians[0]) { +// Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]); +// } +// if (jacobians[1]) { +// Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]); +// } +// if (jacobians[2]) { +// Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]); +// } +template <typename Seq, int ParameterIdx = 0, int Offset = 0> +struct Take1stOrderParts; - typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT; - FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( - N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); - - // These are the positions of the respective jets in the fixed array x. - const int jet0 = 0; - const int jet1 = N0; - const int jet2 = N0 + N1; - const int jet3 = N0 + N1 + N2; - const int jet4 = N0 + N1 + N2 + N3; - const int jet5 = N0 + N1 + N2 + N3 + N4; - const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; - const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; - const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; - const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; - - const JetT *unpacked_parameters[10] = { - x.get() + jet0, - x.get() + jet1, - x.get() + jet2, - x.get() + jet3, - x.get() + jet4, - x.get() + jet5, - x.get() + jet6, - x.get() + jet7, - x.get() + jet8, - x.get() + jet9, - }; - - JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; - - // Invalidate the output Jets, so that we can detect if the user - // did not assign values to all of them. - for (int i = 0; i < num_outputs; ++i) { - output[i].a = kImpossibleValue; - output[i].v.setConstant(kImpossibleValue); +template <int N, int... Ns, int ParameterIdx, int Offset> +struct Take1stOrderParts<integer_sequence<int, N, Ns...>, ParameterIdx, + Offset> { + template <typename JetT, typename T> + static void Apply(int num_outputs, JetT* output, T** jacobians) { + if (jacobians[ParameterIdx]) { + Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]); } - -#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ - if (N ## i) { \ - internal::Make1stOrderPerturbation<JetT, T, N ## i>( \ - jet ## i, \ - parameters[i], \ - x.get() + jet ## i); \ - } - CERES_MAKE_1ST_ORDER_PERTURBATION(0); - CERES_MAKE_1ST_ORDER_PERTURBATION(1); - CERES_MAKE_1ST_ORDER_PERTURBATION(2); - CERES_MAKE_1ST_ORDER_PERTURBATION(3); - CERES_MAKE_1ST_ORDER_PERTURBATION(4); - CERES_MAKE_1ST_ORDER_PERTURBATION(5); - CERES_MAKE_1ST_ORDER_PERTURBATION(6); - CERES_MAKE_1ST_ORDER_PERTURBATION(7); - CERES_MAKE_1ST_ORDER_PERTURBATION(8); - CERES_MAKE_1ST_ORDER_PERTURBATION(9); -#undef CERES_MAKE_1ST_ORDER_PERTURBATION - - if (!VariadicEvaluate<Functor, JetT, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( - functor, unpacked_parameters, output)) { - return false; - } - - internal::Take0thOrderPart(num_outputs, output, function_value); - -#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ - if (N ## i) { \ - if (jacobians[i]) { \ - internal::Take1stOrderPart<JetT, T, \ - jet ## i, \ - N ## i>(num_outputs, \ - output, \ - jacobians[i]); \ - } \ - } - CERES_TAKE_1ST_ORDER_PERTURBATION(0); - CERES_TAKE_1ST_ORDER_PERTURBATION(1); - CERES_TAKE_1ST_ORDER_PERTURBATION(2); - CERES_TAKE_1ST_ORDER_PERTURBATION(3); - CERES_TAKE_1ST_ORDER_PERTURBATION(4); - CERES_TAKE_1ST_ORDER_PERTURBATION(5); - CERES_TAKE_1ST_ORDER_PERTURBATION(6); - CERES_TAKE_1ST_ORDER_PERTURBATION(7); - CERES_TAKE_1ST_ORDER_PERTURBATION(8); - CERES_TAKE_1ST_ORDER_PERTURBATION(9); -#undef CERES_TAKE_1ST_ORDER_PERTURBATION - return true; + Take1stOrderParts<integer_sequence<int, Ns...>, ParameterIdx + 1, + Offset + N>::Apply(num_outputs, output, jacobians); } }; +// End of 'recursion'. Nothing more to do. +template <int ParameterIdx, int Offset> +struct Take1stOrderParts<integer_sequence<int>, ParameterIdx, Offset> { + template <typename T, typename JetT> + static void Apply(int /* NOT USED*/, JetT* /* NOT USED*/, + T** /* NOT USED */) {} +}; + +template <typename ParameterDims, typename Functor, typename T> +inline bool AutoDifferentiate(const Functor& functor, + T const *const *parameters, + int num_outputs, + T* function_value, + T** jacobians) { + DCHECK_GT(num_outputs, 0); + + typedef Jet<T, ParameterDims::kNumParameters> JetT; + FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(ParameterDims::kNumParameters + + num_outputs); + + using Parameters = typename ParameterDims::Parameters; + + // These are the positions of the respective jets in the fixed array x. + std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters = + ParameterDims::GetUnpackedParameters(x.get()); + JetT* output = x.get() + ParameterDims::kNumParameters; + + // Invalidate the output Jets, so that we can detect if the user + // did not assign values to all of them. + for (int i = 0; i < num_outputs; ++i) { + output[i].a = kImpossibleValue; + output[i].v.setConstant(kImpossibleValue); + } + + Make1stOrderPerturbations<Parameters>::Apply(parameters, x.get()); + + if (!VariadicEvaluate<ParameterDims>(functor, unpacked_parameters.data(), + output)) { + return false; + } + + Take0thOrderPart(num_outputs, output, function_value); + Take1stOrderParts<Parameters>::Apply(num_outputs, output, jacobians); + + return true; +} + } // namespace internal } // namespace ceres
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h index ab1abc0..f24fdb3 100644 --- a/include/ceres/internal/numeric_diff.h +++ b/include/ceres/internal/numeric_diff.h
@@ -50,42 +50,11 @@ namespace ceres { namespace internal { -// Helper templates that allow evaluation of a variadic functor or a -// CostFunction object. -template <typename CostFunctor, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9 > -bool EvaluateImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const void* /* NOT USED */) { - return VariadicEvaluate<CostFunctor, - double, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( - *functor, - parameters, - residuals); -} - -template <typename CostFunctor, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9 > -bool EvaluateImpl(const CostFunctor* functor, - double const* const* parameters, - double* residuals, - const CostFunction* /* NOT USED */) { - return functor->Evaluate(parameters, residuals, NULL); -} - // This is split from the main class because C++ doesn't allow partial template // specializations for member functions. The alternative is to repeat the main // class for differing numbers of parameters, which is also unfortunate. -template <typename CostFunctor, - NumericDiffMethodType kMethod, - int kNumResiduals, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9, - int kParameterBlock, +template <typename CostFunctor, NumericDiffMethodType kMethod, + int kNumResiduals, typename ParameterDims, int kParameterBlock, int kParameterBlockSize> struct NumericDiff { // Mutates parameters but must restore them before return. @@ -219,8 +188,9 @@ // Mutate 1 element at a time and then restore. x_plus_delta(parameter_index) = x(parameter_index) + delta; - if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor, parameters, residuals.data(), functor)) { + if (!VariadicEvaluate<ParameterDims>(*functor, + parameters, + residuals.data())) { return false; } @@ -233,8 +203,9 @@ // Compute the function on the other side of x(parameter_index). x_plus_delta(parameter_index) = x(parameter_index) - delta; - if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor, parameters, temp_residuals.data(), functor)) { + if (!VariadicEvaluate<ParameterDims>(*functor, + parameters, + temp_residuals.data())) { return false; } @@ -406,35 +377,91 @@ } }; -template <typename CostFunctor, - NumericDiffMethodType kMethod, - int kNumResiduals, - int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9, - int kParameterBlock> -struct NumericDiff<CostFunctor, kMethod, kNumResiduals, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, - kParameterBlock, 0> { - // Mutates parameters but must restore them before return. - static bool EvaluateJacobianForParameterBlock( - const CostFunctor* functor, - const double* residuals_at_eval_point, - const NumericDiffOptions& options, - const int num_residuals, - const int parameter_block_index, - const int parameter_block_size, - double **parameters, - double *jacobian) { - // Silence unused parameter compiler warnings. - (void)functor; - (void)residuals_at_eval_point; - (void)options; - (void)num_residuals; - (void)parameter_block_index; - (void)parameter_block_size; - (void)parameters; - (void)jacobian; - LOG(FATAL) << "Control should never reach here."; +// This function calls NumericDiff<...>::EvaluateJacobianForParameterBlock for +// each parameter block. +// +// Example: +// A call to +// EvaluateJacobianForParameterBlocks<StaticParameterDims<2, 3>>( +// functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// parameters, +// jacobians); +// will result in the following calls to +// NumericDiff<...>::EvaluateJacobianForParameterBlock: +// +// if (!NumericDiff< +// CostFunctor, method, kNumResiduals, ParameterDims, 0, +// 2>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 0, +// 2, +// parameters, +// jacobians[0])) { +// return false; +// } +// if (!NumericDiff< +// CostFunctor, method, kNumResiduals, ParameterDims, 1, +// 3>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 1, +// 3, +// parameters, +// jacobians[1])) { +// return false; +// } +template <typename ParameterDims, + typename Parameters = typename ParameterDims::Parameters, + int ParameterIdx = 0> +struct EvaluateJacobianForParameterBlocks; + +template <typename ParameterDims, int N, int... Ns, int ParameterIdx> +struct EvaluateJacobianForParameterBlocks< + ParameterDims, integer_sequence<int, N, Ns...>, ParameterIdx> { + template <NumericDiffMethodType method, int kNumResiduals, + typename CostFunctor> + static bool Apply(const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, int num_residuals, + double** parameters, double** jacobians) { + if (!NumericDiff< + CostFunctor, method, kNumResiduals, ParameterDims, ParameterIdx, + N>::EvaluateJacobianForParameterBlock(functor, + residuals_at_eval_point, + options, + num_residuals, + ParameterIdx, + N, + parameters, + jacobians[ParameterIdx])) { + return false; + } + return EvaluateJacobianForParameterBlocks< + ParameterDims, integer_sequence<int, Ns...>, ParameterIdx + 1>:: + template Apply<method, kNumResiduals>(functor, + residuals_at_eval_point, + options, + num_residuals, + parameters, jacobians); + } +}; + +// End of 'recursion'. Nothing more to do. +template <typename ParameterDims, int ParameterIdx> +struct EvaluateJacobianForParameterBlocks<ParameterDims, integer_sequence<int>, + ParameterIdx> { + template <NumericDiffMethodType method, int kNumResiduals, + typename CostFunctor> + static bool Apply(const CostFunctor* /* NOT USED*/, + const double* /* NOT USED*/, + const NumericDiffOptions& /* NOT USED*/, int /* NOT USED*/, + double** /* NOT USED*/, double** /* NOT USED*/) { return true; } };
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h index b3515b9..26428d0 100644 --- a/include/ceres/internal/variadic_evaluate.h +++ b/include/ceres/internal/variadic_evaluate.h
@@ -28,165 +28,76 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // mierle@gmail.com (Keir Mierle) +// jodebo_beck@gmx.de (Johannes Beck) #ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #include <stddef.h> -#include "ceres/jet.h" -#include "ceres/types.h" -#include "ceres/internal/eigen.h" -#include "ceres/internal/fixed_array.h" -#include "glog/logging.h" +#include <type_traits> + +#include "ceres/cost_function.h" +#include "ceres/internal/parameter_dims.h" namespace ceres { namespace internal { -// This block of quasi-repeated code calls the user-supplied functor, which may -// take a variable number of arguments. This is accomplished by specializing the -// struct based on the size of the trailing parameters; parameters with 0 size -// are assumed missing. -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8, int N9> -struct VariadicEvaluate { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - input[8], - input[9], - output); - } -}; +// For fixed size cost functors +template <typename Functor, typename T, int... Indices> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, std::false_type /*is_dynamic*/, + integer_sequence<int, Indices...>) { + static_assert(sizeof...(Indices), + "Invalid number of parameter blocks. At least one parameter " + "block must be specified."); + return functor(input[Indices]..., output); +} -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7, int N8> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - input[8], - output); - } -}; +// For dynamic sized cost functors +template <typename Functor, typename T> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, std::true_type /*is_dynamic*/, + integer_sequence<int>) { + return functor(input, output); +} -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6, int N7> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - input[7], - output); - } -}; +// For ceres cost functors (not ceres::CostFunction) +template <typename ParameterDims, typename Functor, typename T> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, const void* /* NOT USED */) { + using ParameterBlockIndices = + make_integer_sequence<int, ParameterDims::kNumParameterBlocks>; + using IsDynamic = std::integral_constant<bool, ParameterDims::kIsDynamic>; + return VariadicEvaluateImpl(functor, input, output, IsDynamic(), + ParameterBlockIndices()); +} -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5, int N6> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - input[6], - output); - } -}; +// For ceres::CostFunction +template <typename ParameterDims, typename Functor, typename T> +inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input, + T* output, + const CostFunction* /* NOT USED */) { + return functor.Evaluate(input, output, nullptr); +} -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, - int N5> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - input[5], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - input[4], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2, int N3> -struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - input[3], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1, int N2> -struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - input[2], - output); - } -}; - -template<typename Functor, typename T, int N0, int N1> -struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - input[1], - output); - } -}; - -template<typename Functor, typename T, int N0> -struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input[0], - output); - } -}; - -// Template instantiation for dynamically-sized functors. -template<typename Functor, typename T> -struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC, - ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC, - ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC, - ceres::DYNAMIC, ceres::DYNAMIC> { - static bool Call(const Functor& functor, T const *const *input, T* output) { - return functor(input, output); - } -}; +// Variadic evaluate is a helper function to evaluate ceres cost function or +// functors using an input, output and the parameter dimensions. There are +// several ways different possibilities: +// 1) If the passed functor is a 'ceres::CostFunction' its evaluate method is +// called. +// 2) If the functor is not a 'ceres::CostFunction' and the specified parameter +// dims is dynamic, the functor must have the following signature +// 'bool(T const* const* input, T* output)'. +// 3) If the functor is not a 'ceres::CostFunction' and the specified parameter +// dims is not dynamic, the input is expanded by using the number of parameter +// blocks. The signature of the functor must have the following signature +// 'bool()(const T* i_1, const T* i_2, ... const T* i_n, T* output)'. +template <typename ParameterDims, typename Functor, typename T> +inline bool VariadicEvaluate(const Functor& functor, T const* const* input, + T* output) { + return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor); +} } // namespace internal } // namespace ceres
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h index 6ec86fa..f6cd32a 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h
@@ -52,8 +52,8 @@ // The actual cost added to the total problem is e^2, or (k - x'k)^2; however, // the squaring is implicitly done by the optimization framework. // -// To write an numerically-differentiable cost function for the above model, first -// define the object +// To write an numerically-differentiable cost function for the above model, +// first define the object // // class MyScalarCostFunctor { // explicit MyScalarCostFunctor(double k): k_(k) {} @@ -110,10 +110,6 @@ // 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. -// // The central difference method is considerably more accurate at the cost of // twice as many function evaluations than forward difference. Consider using // central differences begin with, and only after that works, trying forward @@ -161,10 +157,13 @@ #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#include <array> #include <memory> + #include "Eigen/Dense" #include "ceres/cost_function.h" #include "ceres/internal/numeric_diff.h" +#include "ceres/internal/parameter_dims.h" #include "ceres/numeric_diff_options.h" #include "ceres/sized_cost_function.h" #include "ceres/types.h" @@ -175,20 +174,8 @@ template <typename CostFunctor, NumericDiffMethodType method = CENTRAL, int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC - int N0 = 0, // 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<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> { + int... Ns> // Parameters dimensions for each block. +class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { public: NumericDiffCostFunction( CostFunctor* functor, @@ -199,10 +186,7 @@ ownership_(ownership), options_(options) { if (kNumResiduals == DYNAMIC) { - SizedCostFunction<kNumResiduals, - N0, N1, N2, N3, N4, - N5, N6, N7, N8, N9> - ::set_num_residuals(num_residuals); + SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals); } } @@ -218,18 +202,17 @@ using internal::FixedArray; using internal::NumericDiff; - const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; - const int kNumParameterBlocks = - (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + - (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0); + using ParameterDims = + typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims; + using Parameters = typename ParameterDims::Parameters; - // Get the function value (residuals) at the point to evaluate. - if (!internal::EvaluateImpl<CostFunctor, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( - functor_.get(), - parameters, - residuals, - functor_.get())) { + constexpr int kNumParameters = ParameterDims::kNumParameters; + constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks; + + // Get the function value (residuals) at the the point to evaluate. + if (!internal::VariadicEvaluate<ParameterDims>(*functor_, + parameters, + residuals)) { return false; } @@ -239,71 +222,22 @@ // Create a copy of the parameters which will get mutated. FixedArray<double> parameters_copy(kNumParameters); - FixedArray<double*> parameters_reference_copy(kNumParameterBlocks); + std::array<double*, kNumParameterBlocks> parameters_reference_copy = + ParameterDims::GetUnpackedParameters(parameters_copy.get()); - parameters_reference_copy[0] = parameters_copy.get(); - if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0; - if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1; - if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2; - if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3; - if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4; - if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5; - if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6; - if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7; - if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8; - -#define CERES_COPY_PARAMETER_BLOCK(block) \ - if (N ## block) memcpy(parameters_reference_copy[block], \ - parameters[block], \ - sizeof(double) * N ## block); // NOLINT - - CERES_COPY_PARAMETER_BLOCK(0); - CERES_COPY_PARAMETER_BLOCK(1); - CERES_COPY_PARAMETER_BLOCK(2); - CERES_COPY_PARAMETER_BLOCK(3); - CERES_COPY_PARAMETER_BLOCK(4); - CERES_COPY_PARAMETER_BLOCK(5); - CERES_COPY_PARAMETER_BLOCK(6); - CERES_COPY_PARAMETER_BLOCK(7); - CERES_COPY_PARAMETER_BLOCK(8); - CERES_COPY_PARAMETER_BLOCK(9); - -#undef CERES_COPY_PARAMETER_BLOCK - -#define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block) \ - if (N ## block && jacobians[block] != NULL) { \ - if (!NumericDiff<CostFunctor, \ - method, \ - kNumResiduals, \ - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, \ - block, \ - N ## block >::EvaluateJacobianForParameterBlock( \ - functor_.get(), \ - residuals, \ - options_, \ - SizedCostFunction<kNumResiduals, \ - N0, N1, N2, N3, N4, \ - N5, N6, N7, N8, N9>::num_residuals(), \ - block, \ - N ## block, \ - parameters_reference_copy.get(), \ - jacobians[block])) { \ - return false; \ - } \ + for (int block = 0; block < kNumParameterBlocks; ++block) { + memcpy(parameters_reference_copy[block], parameters[block], + sizeof(double) * ParameterDims::GetDim(block)); } - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8); - CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9); - -#undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK + internal::EvaluateJacobianForParameterBlocks<ParameterDims>::template Apply< + method, kNumResiduals>( + functor_.get(), + residuals, + options_, + SizedCostFunction<kNumResiduals, Ns...>::num_residuals(), + parameters_reference_copy.data(), + jacobians); return true; }
diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h index f9a984f..50d0363 100644 --- a/include/ceres/sized_cost_function.h +++ b/include/ceres/sized_cost_function.h
@@ -41,49 +41,24 @@ #include "ceres/cost_function.h" #include "ceres/types.h" #include "glog/logging.h" +#include "internal/parameter_dims.h" namespace ceres { -template<int kNumResiduals, - 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 <int kNumResiduals, int... Ns> class SizedCostFunction : public CostFunction { public: + static_assert(kNumResiduals > 0 || kNumResiduals == DYNAMIC, + "Cost functions must have at least one residual block."); + static_assert(internal::StaticParameterDims<Ns...>::kIsValid, + "Invalid parameter block dimension detected. Each parameter " + "block dimension must be bigger than zero."); + + using ParameterDims = internal::StaticParameterDims<Ns...>; + SizedCostFunction() { - CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC) - << "Cost functions must have at least one residual block."; - - // This block breaks the 80 column rule to keep it somewhat readable. - CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT - << "Zero block cannot precede a non-zero block. Block sizes are " - << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " - << N8 << ", " << N9; - set_num_residuals(kNumResiduals); - -#define CERES_ADD_PARAMETER_BLOCK(N) \ - if (N) mutable_parameter_block_sizes()->push_back(N); - CERES_ADD_PARAMETER_BLOCK(N0); - CERES_ADD_PARAMETER_BLOCK(N1); - CERES_ADD_PARAMETER_BLOCK(N2); - CERES_ADD_PARAMETER_BLOCK(N3); - CERES_ADD_PARAMETER_BLOCK(N4); - CERES_ADD_PARAMETER_BLOCK(N5); - CERES_ADD_PARAMETER_BLOCK(N6); - CERES_ADD_PARAMETER_BLOCK(N7); - CERES_ADD_PARAMETER_BLOCK(N8); - CERES_ADD_PARAMETER_BLOCK(N9); -#undef CERES_ADD_PARAMETER_BLOCK + *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...}; } virtual ~SizedCostFunction() { }
diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h index 76e4a24..cc93d73 100644 --- a/include/ceres/tiny_solver_autodiff_function.h +++ b/include/ceres/tiny_solver_autodiff_function.h
@@ -120,7 +120,7 @@ NUM_RESIDUALS = kNumResiduals, }; - // This is similar to AutoDiff::Differentiate(), but since there is only one + // This is similar to AutoDifferentiate(), but since there is only one // parameter block it is easier to inline to avoid overhead. bool operator()(const T* parameters, T* residuals,
diff --git a/internal/ceres/autodiff_test.cc b/internal/ceres/autodiff_test.cc index 20f4437..04a77ea 100644 --- a/internal/ceres/autodiff_test.cc +++ b/internal/ceres/autodiff_test.cc
@@ -194,7 +194,7 @@ { double *parameters[] = { PX }; double *jacobians[] = { J_PX }; - ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate( + ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12 + 4>>( b, parameters, 2, ad_x1, jacobians))); for (int i = 0; i < 2; ++i) { @@ -209,7 +209,7 @@ double J_X[2 * 4]; double *parameters[] = { P, X }; double *jacobians[] = { J_P, J_X }; - ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate( + ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12, 4>>( b, parameters, 2, ad_x2, jacobians))); for (int i = 0; i < 2; ++i) { @@ -316,7 +316,7 @@ double J_X[2 * 3]; double *parameters[] = { q, c, X }; double *jacobians[] = { J_q, J_c, J_X }; - ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate( + ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<4, 3, 3>>( b, parameters, 2, ad_x, jacobians))); for (int i = 0; i < 2; ++i) { @@ -366,7 +366,7 @@ functor.num_residuals = num_residuals; // Run autodiff with the new number of residuals. - ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate( + ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<2>>( functor, parameters, num_residuals, residuals, jacobians))); const double kTolerance = 1e-14; @@ -528,8 +528,8 @@ { Residual1Param functor; int num_variables = 1; - EXPECT_TRUE((AutoDiff<Residual1Param, double, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -539,8 +539,8 @@ { Residual2Param functor; int num_variables = 2; - EXPECT_TRUE((AutoDiff<Residual2Param, double, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -550,8 +550,8 @@ { Residual3Param functor; int num_variables = 3; - EXPECT_TRUE((AutoDiff<Residual3Param, double, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -561,8 +561,8 @@ { Residual4Param functor; int num_variables = 4; - EXPECT_TRUE((AutoDiff<Residual4Param, double, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -572,8 +572,8 @@ { Residual5Param functor; int num_variables = 5; - EXPECT_TRUE((AutoDiff<Residual5Param, double, 1, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -583,10 +583,8 @@ { Residual6Param functor; int num_variables = 6; - EXPECT_TRUE((AutoDiff<Residual6Param, - double, - 1, 1, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -596,10 +594,8 @@ { Residual7Param functor; int num_variables = 7; - EXPECT_TRUE((AutoDiff<Residual7Param, - double, - 1, 1, 1, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -609,10 +605,8 @@ { Residual8Param functor; int num_variables = 8; - EXPECT_TRUE((AutoDiff< - Residual8Param, - double, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -622,11 +616,9 @@ { Residual9Param functor; int num_variables = 9; - EXPECT_TRUE((AutoDiff< - Residual9Param, - double, - 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE( + (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); @@ -636,11 +628,9 @@ { Residual10Param functor; int num_variables = 10; - EXPECT_TRUE((AutoDiff< - Residual10Param, - double, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate( - functor, parameters, 1, &residual, jacobians))); + EXPECT_TRUE( + (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1, 1>>( + functor, parameters, 1, &residual, jacobians))); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); for (int i = 0; i < num_variables; ++i) { EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index a7833d8..18b7e8c 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/internal/ceres/local_parameterization_test.cc
@@ -265,14 +265,12 @@ double* jacobian_array[2] = { NULL, jacobian_ref }; // Autodiff jacobian at delta_x = 0. - internal::AutoDiff<Plus, - double, - kGlobalSize, - kLocalSize>::Differentiate(Plus(), - parameters, - kGlobalSize, - x_plus_delta, - jacobian_array); + internal::AutoDifferentiate<StaticParameterDims<kGlobalSize, kLocalSize>>( + Plus(), + parameters, + kGlobalSize, + x_plus_delta, + jacobian_array); double jacobian[12]; parameterization.ComputeJacobian(x, jacobian);
diff --git a/internal/ceres/parameter_dims_test.cc b/internal/ceres/parameter_dims_test.cc index 9a95cf5..f33536f 100644 --- a/internal/ceres/parameter_dims_test.cc +++ b/internal/ceres/parameter_dims_test.cc
@@ -89,11 +89,11 @@ constexpr int N1 = 4; constexpr int N2 = 2; - using Params = StaticParameterDims<N0, N1, N2>; + using ParameterDims = StaticParameterDims<N0, N1, N2>; - std::array<double, Params::kNumParameters> packed_parameters{}; + std::array<double, ParameterDims::kNumParameters> packed_parameters{}; std::array<double*, 3> unpacked_parameters = - Params::GetUnpackedParameters(packed_parameters.data()); + ParameterDims::GetUnpackedParameters(packed_parameters.data()); EXPECT_EQ(packed_parameters.data(), unpacked_parameters[0]); EXPECT_EQ(packed_parameters.data() + N0, unpacked_parameters[1]);