diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index 8e576016f..29a17d8db 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h @@ -177,64 +177,35 @@ namespace ceres::internal { // // is what would get put in dst if N was 3, offset was 3, and the jet type JetT // was 8-dimensional. -template -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::Apply(src, dst); - } -}; - template -struct Make1stOrderPerturbation { - 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::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 -struct Make1stOrderPerturbations; - -template -struct Make1stOrderPerturbations, - ParameterIdx, - Offset> { - template - inline static void Apply(T const* const* parameters, JetT* x) { - Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply( - parameters[ParameterIdx], x + Offset); - Make1stOrderPerturbations, - 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 -struct Make1stOrderPerturbations, - ParameterIdx, - Total> { - template - static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {} -}; +// Internal non-recursive implementation of Make1stOrderPerturbations. +template +inline void Make1stOrderPerturbationsImpl( + T const* const* parameters, + JetT* x, + std::integer_sequence, + std::integer_sequence, + std::index_sequence) { + ((Make1stOrderPerturbation(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 -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 @@ inline void Take1stOrderPart(const int M, const JetT* src, T* dst) { } } -// Calls Take1stOrderPart for every parameter block. -// -// Example: -// If one having three parameter blocks with dimensions (3, 2, 4), the call -// Take1stOrderParts::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 -struct Take1stOrderParts; - -template -struct Take1stOrderParts, - ParameterIdx, - Offset> { - template - inline static void Apply(int num_outputs, JetT* output, T** jacobians) { - if (jacobians[ParameterIdx]) { - Take1stOrderPart(num_outputs, output, jacobians[ParameterIdx]); - } - Take1stOrderParts, - ParameterIdx + 1, - Offset + N>::Apply(num_outputs, output, jacobians); - } -}; - -// End of 'recursion'. Nothing more to do. -template -struct Take1stOrderParts, ParameterIdx, Offset> { - template - static void Apply(int /* NOT USED*/, - JetT* /* NOT USED*/, - T** /* NOT USED */) {} -}; +// Internal non-recursive implementation of Take1stOrderParts. +template +inline void Take1stOrderPartsImpl(int num_outputs, + JetT* output, + T** jacobians, + std::integer_sequence, + std::integer_sequence, + std::index_sequence) { + ((void)(jacobians[BlockIdx] != nullptr && + (Take1stOrderPart( + num_outputs, output, jacobians[BlockIdx]), + true)), + ...); +} template 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::Apply(parameters, - parameters_as_jets.data()); + using Offsets = ExclusiveScan; + Make1stOrderPerturbationsImpl( + parameters, + parameters_as_jets.data(), + Parameters{}, + Offsets{}, + std::make_index_sequence{}); if (!VariadicEvaluate( functor, unpacked_parameters.data(), residuals_as_jets.data())) { @@ -349,8 +298,13 @@ inline bool AutoDifferentiate(const Functor& functor, } Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value); - Take1stOrderParts::Apply( - num_outputs, residuals_as_jets.data(), jacobians); + Take1stOrderPartsImpl( + num_outputs, + residuals_as_jets.data(), + jacobians, + Parameters{}, + Offsets{}, + std::make_index_sequence{}); return true; } diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h index 0fe4b8278..32d3ea27f 100644 --- a/include/ceres/internal/numeric_diff.h +++ b/include/ceres/internal/numeric_diff.h @@ -341,11 +341,12 @@ struct NumericDiff { 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 @@ struct NumericDiff { // return false; // } // } -template -struct EvaluateJacobianForParameterBlocks; +// Internal non-recursive implementation of EvaluateJacobianForParameterBlocks. +template +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, + std::index_sequence) { + 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 -struct EvaluateJacobianForParameterBlocks, - ParameterIdx> { - template - 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:: - 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, - ParameterIdx + 1>::template Apply(functor, - residuals_at_eval_point, - options, - num_residuals, - parameters, - jacobians); - } - return true; - } -}; +template +inline bool EvaluateJacobianForParameterBlocks( + const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + double** parameters, + double** jacobians) { + return EvaluateJacobianForParameterBlocksImpl( + functor, + residuals_at_eval_point, + options, + num_residuals, + parameters, + jacobians, + Seq{}, + std::make_index_sequence{}); +} } // namespace ceres::internal diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h index 182242267..89ddda7e5 100644 --- a/include/ceres/numeric_diff_cost_function.h +++ b/include/ceres/numeric_diff_cost_function.h @@ -266,14 +266,15 @@ class NumericDiffCostFunction final sizeof(double) * ParameterDims::GetDim(block)); } - internal::EvaluateJacobianForParameterBlocks:: - template Apply( - functor_.get(), - residuals, - options_, - this->num_residuals(), - parameters_reference_copy.data(), - jacobians); + internal::EvaluateJacobianForParameterBlocks( + 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 f136e31a9..b6f8c34ed 100644 --- a/include/ceres/numeric_diff_first_order_function.h +++ b/include/ceres/numeric_diff_first_order_function.h @@ -148,9 +148,10 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction { explicit NumericDiffFirstOrderFunction(FirstOrderFunctor* functor, Ownership ownership = TAKE_OWNERSHIP) - : NumericDiffFirstOrderFunction(std::unique_ptr(functor), - kNumParameters, - ownership) { + : NumericDiffFirstOrderFunction( + std::unique_ptr(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 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction { ¶meters_ptr, gradient); } else { - return internal::EvaluateJacobianForParameterBlocks< - internal::StaticParameterDims>:: - template Apply(functor_.get(), - cost, - options_, - kNumResiduals, - ¶meters_ptr, - &gradient); + using ParameterDims = internal::StaticParameterDims; + return internal::EvaluateJacobianForParameterBlocks( + functor_.get(), + cost, + options_, + kNumResiduals, + ¶meters_ptr, + &gradient); } } @@ -225,10 +228,11 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction { const FirstOrderFunctor& functor() const { return *functor_; } private: - explicit NumericDiffFirstOrderFunction(std::unique_ptr functor, - Ownership ownership, - int num_parameters, - const NumericDiffOptions& options) + explicit NumericDiffFirstOrderFunction( + std::unique_ptr functor, + Ownership ownership, + int num_parameters, + const NumericDiffOptions& options) : functor_(std::move(functor)), num_parameters_(num_parameters), ownership_(ownership),