[modernize] Modernize internal autodiff and numeric_diff logic Refactor the core automatic and numeric differentiation internal helpers to utilize C++17 features for improved readability and maintainability: - Added IntegerSequenceTraits to provide a uniform compile-time interface for accessing head and tail of std::integer_sequence. - Eliminated recursive template meta-programming in autodiff.h, replacing it with fold expressions and std::index_sequence. - Simplified core helper functions like Make1stOrderPerturbation into unrolled loops. - Refactored EvaluateJacobianForParameterBlocks from a recursive struct to a recursive function template using if constexpr. - Updated NumericDiffCostFunction and NumericDiffFirstOrderFunction to use the modernized internal helper API. Change-Id: I934034f7434fc05a8855565b2b534d325f920584
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index 8e57601..29a17d8 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h
@@ -177,64 +177,35 @@ // // is what would get put in dst if N was 3, offset was 3, and the jet type JetT // was 8-dimensional. -template <int j, int N, int Offset, typename T, typename JetT> -struct Make1stOrderPerturbation { - public: - inline static void Apply(const T* src, JetT* dst) { - if (j == 0) { - DCHECK(src); - DCHECK(dst); - } - dst[j] = JetT(src[j], j + Offset); - Make1stOrderPerturbation<j + 1, N, Offset, T, JetT>::Apply(src, dst); - } -}; - template <int N, int Offset, typename T, typename JetT> -struct Make1stOrderPerturbation<N, N, Offset, T, JetT> { - public: - static void Apply(const T* /* NOT USED */, JetT* /* NOT USED */) {} -}; - -// 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, 0>::Apply(params[0], x + 0); -// Make1stOrderPerturbation<0, 2, 3>::Apply(params[1], x + 3); -// Make1stOrderPerturbation<0, 4, 5>::Apply(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<std::integer_sequence<int, N, Ns...>, - ParameterIdx, - Offset> { - template <typename T, typename JetT> - inline static void Apply(T const* const* parameters, JetT* x) { - Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply( - parameters[ParameterIdx], x + Offset); - Make1stOrderPerturbations<std::integer_sequence<int, Ns...>, - ParameterIdx + 1, - Offset + N>::Apply(parameters, x); +inline void Make1stOrderPerturbation(const T* src, JetT* dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < N; ++i) { + dst[i] = JetT(src[i], i + Offset); } -}; +} -// End of 'recursion'. Nothing more to do. -template <int ParameterIdx, int Total> -struct Make1stOrderPerturbations<std::integer_sequence<int>, - ParameterIdx, - Total> { - template <typename T, typename JetT> - static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {} -}; +// Internal non-recursive implementation of Make1stOrderPerturbations. +template <int... Ns, + int... Offsets, + std::size_t... BlockIdx, + typename T, + typename JetT> +inline void Make1stOrderPerturbationsImpl( + T const* const* parameters, + JetT* x, + std::integer_sequence<int, Ns...>, + std::integer_sequence<int, Offsets...>, + std::index_sequence<BlockIdx...>) { + ((Make1stOrderPerturbation<Ns, Offsets>(parameters[BlockIdx], x + Offsets)), + ...); +} // 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; @@ -253,49 +224,24 @@ } } -// 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; - -template <int N, int... Ns, int ParameterIdx, int Offset> -struct Take1stOrderParts<std::integer_sequence<int, N, Ns...>, - ParameterIdx, - Offset> { - template <typename JetT, typename T> - inline static void Apply(int num_outputs, JetT* output, T** jacobians) { - if (jacobians[ParameterIdx]) { - Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]); - } - Take1stOrderParts<std::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<std::integer_sequence<int>, ParameterIdx, Offset> { - template <typename T, typename JetT> - static void Apply(int /* NOT USED*/, - JetT* /* NOT USED*/, - T** /* NOT USED */) {} -}; +// Internal non-recursive implementation of Take1stOrderParts. +template <int... Ns, + int... Offsets, + std::size_t... BlockIdx, + typename JetT, + typename T> +inline void Take1stOrderPartsImpl(int num_outputs, + JetT* output, + T** jacobians, + std::integer_sequence<int, Ns...>, + std::integer_sequence<int, Offsets...>, + std::index_sequence<BlockIdx...>) { + ((void)(jacobians[BlockIdx] != nullptr && + (Take1stOrderPart<Offsets, Ns>( + num_outputs, output, jacobians[BlockIdx]), + true)), + ...); +} template <int kNumResiduals, typename ParameterDims, @@ -333,15 +279,18 @@ ArraySelector<JetT, kNumResiduals, CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK> residuals_as_jets(num_outputs); - // 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) { residuals_as_jets[i].a = kImpossibleValue; residuals_as_jets[i].v.setConstant(kImpossibleValue); } - Make1stOrderPerturbations<Parameters>::Apply(parameters, - parameters_as_jets.data()); + using Offsets = ExclusiveScan<Parameters>; + Make1stOrderPerturbationsImpl( + parameters, + parameters_as_jets.data(), + Parameters{}, + Offsets{}, + std::make_index_sequence<ParameterDims::kNumParameterBlocks>{}); if (!VariadicEvaluate<ParameterDims>( functor, unpacked_parameters.data(), residuals_as_jets.data())) { @@ -349,8 +298,13 @@ } Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value); - Take1stOrderParts<Parameters>::Apply( - num_outputs, residuals_as_jets.data(), jacobians); + Take1stOrderPartsImpl( + num_outputs, + residuals_as_jets.data(), + jacobians, + Parameters{}, + Offsets{}, + std::make_index_sequence<ParameterDims::kNumParameterBlocks>{}); return true; }
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h index 0fe4b82..32d3ea2 100644 --- a/include/ceres/internal/numeric_diff.h +++ b/include/ceres/internal/numeric_diff.h
@@ -341,11 +341,12 @@ options.ridders_step_shrink_factor; // Compute the difference between the previous value and the current. - double candidate_error = (std::max)( - (current_candidates->col(k) - current_candidates->col(k - 1)) - .norm(), - (current_candidates->col(k) - previous_candidates->col(k - 1)) - .norm()); + double candidate_error = (std::max)((current_candidates->col(k) - + current_candidates->col(k - 1)) + .norm(), + (current_candidates->col(k) - + previous_candidates->col(k - 1)) + .norm()); // If the error has decreased, update results. if (candidate_error <= norm_error) { @@ -434,57 +435,64 @@ // return false; // } // } -template <typename ParameterDims, - typename Parameters = typename ParameterDims::Parameters, - int ParameterIdx = 0> -struct EvaluateJacobianForParameterBlocks; +// Internal non-recursive implementation of EvaluateJacobianForParameterBlocks. +template <NumericDiffMethodType method, + int kNumResiduals, + typename ParameterDims, + int... Ns, + std::size_t... BlockIdx, + typename CostFunctor> +inline bool EvaluateJacobianForParameterBlocksImpl( + const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + double** parameters, + double** jacobians, + std::integer_sequence<int, Ns...>, + std::index_sequence<BlockIdx...>) { + return (... && + (jacobians[BlockIdx] == nullptr || + NumericDiff< + CostFunctor, + method, + kNumResiduals, + ParameterDims, + BlockIdx, + Ns>::EvaluateJacobianForParameterBlock(functor, + residuals_at_eval_point, + options, + num_residuals, + BlockIdx, + Ns, + parameters, + jacobians[BlockIdx]))); +} -template <typename ParameterDims, int N, int... Ns, int ParameterIdx> -struct EvaluateJacobianForParameterBlocks<ParameterDims, - std::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 (jacobians[ParameterIdx] != nullptr) { - if (!NumericDiff<CostFunctor, - method, - kNumResiduals, - ParameterDims, - ParameterIdx, - N>:: - EvaluateJacobianForParameterBlock(functor, - residuals_at_eval_point, - options, - num_residuals, - ParameterIdx, - N, - parameters, - jacobians[ParameterIdx])) { - return false; - } - } - - if constexpr (sizeof...(Ns) > 0) { - return EvaluateJacobianForParameterBlocks< - ParameterDims, - std::integer_sequence<int, Ns...>, - ParameterIdx + 1>::template Apply<method, kNumResiduals>(functor, - residuals_at_eval_point, - options, - num_residuals, - parameters, - jacobians); - } - return true; - } -}; +template <NumericDiffMethodType method, + int kNumResiduals, + typename ParameterDims, + typename Seq = typename ParameterDims::Parameters, + typename CostFunctor> +inline bool EvaluateJacobianForParameterBlocks( + const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + double** parameters, + double** jacobians) { + return EvaluateJacobianForParameterBlocksImpl<method, + kNumResiduals, + ParameterDims>( + functor, + residuals_at_eval_point, + options, + num_residuals, + parameters, + jacobians, + Seq{}, + std::make_index_sequence<ParameterDims::kNumParameterBlocks>{}); +} } // namespace ceres::internal
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h index 1822422..89ddda7 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h
@@ -266,14 +266,15 @@ sizeof(double) * ParameterDims::GetDim(block)); } - internal::EvaluateJacobianForParameterBlocks<ParameterDims>:: - template Apply<kMethod, kNumResiduals>( - functor_.get(), - residuals, - options_, - this->num_residuals(), - parameters_reference_copy.data(), - jacobians); + internal::EvaluateJacobianForParameterBlocks<kMethod, + kNumResiduals, + ParameterDims>( + functor_.get(), + residuals, + options_, + this->num_residuals(), + parameters_reference_copy.data(), + jacobians); return true; }
diff --git a/include/ceres/numeric_diff_first_order_function.h b/include/ceres/numeric_diff_first_order_function.h index f136e31..b6f8c34 100644 --- a/include/ceres/numeric_diff_first_order_function.h +++ b/include/ceres/numeric_diff_first_order_function.h
@@ -148,9 +148,10 @@ explicit NumericDiffFirstOrderFunction(FirstOrderFunctor* functor, Ownership ownership = TAKE_OWNERSHIP) - : NumericDiffFirstOrderFunction(std::unique_ptr<FirstOrderFunctor>(functor), - kNumParameters, - ownership) { + : NumericDiffFirstOrderFunction( + std::unique_ptr<FirstOrderFunctor>(functor), + kNumParameters, + ownership) { static_assert(kNumParameters != DYNAMIC, "When kNumParameters is DYNAMIC, the number of parameters " "must be provided as a constructor argument."); @@ -209,14 +210,16 @@ ¶meters_ptr, gradient); } else { - return internal::EvaluateJacobianForParameterBlocks< - internal::StaticParameterDims<kNumParameters>>:: - template Apply<kMethod, 1>(functor_.get(), - cost, - options_, - kNumResiduals, - ¶meters_ptr, - &gradient); + using ParameterDims = internal::StaticParameterDims<kNumParameters>; + return internal::EvaluateJacobianForParameterBlocks<kMethod, + kNumResiduals, + ParameterDims>( + functor_.get(), + cost, + options_, + kNumResiduals, + ¶meters_ptr, + &gradient); } } @@ -225,10 +228,11 @@ const FirstOrderFunctor& functor() const { return *functor_; } private: - explicit NumericDiffFirstOrderFunction(std::unique_ptr<FirstOrderFunctor> functor, - Ownership ownership, - int num_parameters, - const NumericDiffOptions& options) + explicit NumericDiffFirstOrderFunction( + std::unique_ptr<FirstOrderFunctor> functor, + Ownership ownership, + int num_parameters, + const NumericDiffOptions& options) : functor_(std::move(functor)), num_parameters_(num_parameters), ownership_(ownership),