Streamline ArraySelector, VariadicEvaluate, and perturbation seeding - In ArraySelector, collapse the two absl::FixedArray specializations into a single specialization, replace release-mode CHECK_EQ with DCHECK, use absl::FixedArray<T, 0> instead of std::vector<T> for the heap fallback, and add a fill-value constructor so residual Jet arrays are initialized to kImpossibleValue in a single pass. - Simplify VariadicEvaluateImpl to expand functor(input[Is]..., output) directly via std::index_sequence, removing the intermediate std::array copy and std::apply/std::invoke wrappers, and mark hot helpers EIGEN_STRONG_INLINE. - In Make1stOrderPerturbation, rely on dst[i].v being zero-initialized on entry by Jet's default constructor (checked with DCHECK) and assign dst[i].a and dst[i].v[i + Offset] in-place instead of constructing a temporary JetT and re-zeroing v. Benchmark (Apple M1 Pro, kAutoDiff, kResidualsAndJacobians): ---------------------------------------------------------------------- Benchmark Before After Change ---------------------------------------------------------------------- SnavelyReprojectionError 136 ns 78.4 ns -42.4% PhotometricError<8> 901 ns 748 ns -17.0% RelativePoseError 248 ns 130 ns -47.6% Brdf 827 ns 255 ns -69.2% ColmapOpenCVReprojectionError 185 ns 61.7 ns -66.6% ColmapFullOpenCVReprojectionError 263 ns 86.2 ns -67.2% ColmapRigReprojectionError 391 ns 134 ns -65.7% ColmapSampsonError 100 ns 86.2 ns -13.8% ColmapFisheyeReprojectionError 209 ns 84.6 ns -59.5% ColmapSphericalReprojectionError 86.1 ns 46.5 ns -46.0% LibmvBrownReprojectionError 225 ns 98.7 ns -56.1% LibmvNukeInvertReprojectionError 228 ns 82.8 ns -63.7% PoseGraph3dError 249 ns 139 ns -44.2% RosenbrockFirstOrderFunctor<4> 10.8 ns 7.96 ns -26.3% RosenbrockFirstOrderFunctor<16> 188 ns 163 ns -13.3% RosenbrockFirstOrderFunctor<24> 399 ns 367 ns -8.0% ---------------------------------------------------------------------- Change-Id: I0d3af3eb31b476a8fc5703bbbd529d3fb292ac98
diff --git a/include/ceres/autodiff_first_order_function.h b/include/ceres/autodiff_first_order_function.h index fbd669a..7661509 100644 --- a/include/ceres/autodiff_first_order_function.h +++ b/include/ceres/autodiff_first_order_function.h
@@ -36,9 +36,9 @@ #include <utility> #include "Eigen/Core" -#include "absl/container/fixed_array.h" #include "ceres/eigen.h" #include "ceres/first_order_function.h" +#include "ceres/internal/autodiff.h" #include "ceres/jet.h" #include "ceres/types.h" @@ -156,12 +156,12 @@ } using JetT = Jet<double, kNumParameters>; - absl::FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(kNumParameters); - for (int i = 0; i < kNumParameters; ++i) { - x[i].a = parameters[i]; - x[i].v.setZero(); - x[i].v[i] = 1.0; - } + internal::ArraySelector<JetT, + kNumParameters, + CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK> + x(kNumParameters); + internal::Make1stOrderPerturbation<kNumParameters, 0>(parameters, + x.data()); JetT output; output.a = kImpossibleValue; @@ -172,7 +172,8 @@ } *cost = output.a; - VectorRef(gradient, kNumParameters) = output.v; + Eigen::Map<Eigen::Matrix<double, kNumParameters, 1>>( + gradient, kNumParameters) = output.v; return true; }
diff --git a/include/ceres/internal/array_selector.h b/include/ceres/internal/array_selector.h index fd56a60..cc940f9 100644 --- a/include/ceres/internal/array_selector.h +++ b/include/ceres/internal/array_selector.h
@@ -35,61 +35,57 @@ // IWYU pragma: private #include <array> -#include <vector> +#include "Eigen/Core" #include "absl/container/fixed_array.h" #include "absl/log/check.h" #include "ceres/types.h" namespace ceres::internal { -// StaticFixedArray selects the best array implementation based on template +// ArraySelector selects the best array implementation based on template // arguments. If the size is not known at compile-time, pass -// ceres::DYNAMIC as a size-template argument. +// ceres::DYNAMIC as the size template argument. // // Three different containers are selected in different scenarios: // // num_elements == DYNAMIC: -// -> absl::FixedArray<T, max_stack_size>(size) - -// num_elements != DYNAMIC && num_elements <= max_stack_size -// -> std::array<T,num_elements> - -// num_elements != DYNAMIC && num_elements > max_stack_size -// -> std::vector<T>(num_elements) +// -> absl::FixedArray<T, max_num_elements_on_stack>(size) +// num_elements != DYNAMIC && num_elements <= max_num_elements_on_stack: +// -> std::array<T, num_elements> +// num_elements != DYNAMIC && num_elements > max_num_elements_on_stack: +// -> absl::FixedArray<T, 0>(num_elements) // template <typename T, int num_elements, int max_num_elements_on_stack, - bool dynamic = (num_elements == DYNAMIC), - bool fits_on_stack = (num_elements <= max_num_elements_on_stack)> -struct ArraySelector {}; - -template <typename T, - int num_elements, - int max_num_elements_on_stack, - bool fits_on_stack> -struct ArraySelector<T, - num_elements, - max_num_elements_on_stack, - true, - fits_on_stack> - : absl::FixedArray<T, max_num_elements_on_stack> { - explicit ArraySelector(int s) - : absl::FixedArray<T, max_num_elements_on_stack>(s) {} + bool fits_on_stack = + (num_elements != DYNAMIC && + num_elements <= max_num_elements_on_stack)> +struct ArraySelector : std::array<T, num_elements> { + EIGEN_STRONG_INLINE explicit ArraySelector(int s) { + DCHECK_EQ(s, num_elements); + } + EIGEN_STRONG_INLINE ArraySelector(int s, const T& init) { + DCHECK_EQ(s, num_elements); + this->fill(init); + } }; template <typename T, int num_elements, int max_num_elements_on_stack> -struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, true> - : std::array<T, num_elements> { - explicit ArraySelector(int s) { CHECK_EQ(s, num_elements); } -}; +struct ArraySelector<T, num_elements, max_num_elements_on_stack, false> + : absl::FixedArray< + T, + num_elements == DYNAMIC ? max_num_elements_on_stack : 0> { + using Base = absl::FixedArray< + T, + num_elements == DYNAMIC ? max_num_elements_on_stack : 0>; -template <typename T, int num_elements, int max_num_elements_on_stack> -struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, false> - : std::vector<T> { - explicit ArraySelector(int s) : std::vector<T>(s) { - CHECK_EQ(s, num_elements); + EIGEN_STRONG_INLINE explicit ArraySelector(int s) : Base(s) { + DCHECK(num_elements == DYNAMIC || s == num_elements); + } + EIGEN_STRONG_INLINE ArraySelector(int s, const T& init) : Base(s, init) { + DCHECK(num_elements == DYNAMIC || s == num_elements); } };
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index 5317aff..e54585a 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h
@@ -180,13 +180,16 @@ // dst[2] [ * | . . | 0 0 1 | . . . ] // // is what would get put in dst if N was 3, offset was 3, and the jet type JetT -// was 8-dimensional. +// was 8-dimensional. The infinitesimal part dst[i].v must be zero-initialized +// on entry (as guaranteed by Jet's default constructor). template <int N, int Offset, typename T, typename JetT> -inline void Make1stOrderPerturbation(const T* src, JetT* dst) { +EIGEN_STRONG_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); + DCHECK(dst[i].v.isZero()); + dst[i].a = src[i]; + dst[i].v[i + Offset] = T(1.0); } } @@ -196,7 +199,7 @@ std::size_t... BlockIdx, typename T, typename JetT> -inline void Make1stOrderPerturbationsImpl( +EIGEN_STRONG_INLINE void Make1stOrderPerturbationsImpl( T const* const* parameters, JetT* x, std::integer_sequence<int, Ns...>, @@ -209,7 +212,7 @@ // 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) { +EIGEN_STRONG_INLINE void Take0thOrderPart(int M, const JetT* src, T* dst) { DCHECK(src); for (int i = 0; i < M; ++i) { dst[i] = src[i].a; @@ -219,7 +222,9 @@ // 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 <int N0, int N, typename JetT, typename T> -inline void Take1stOrderPart(const int M, const JetT* src, T* dst) { +EIGEN_STRONG_INLINE void Take1stOrderPart(const int M, + const JetT* src, + T* dst) { DCHECK(src); DCHECK(dst); for (int i = 0; i < M; ++i) { @@ -234,12 +239,13 @@ 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...>) { +EIGEN_STRONG_INLINE void Take1stOrderPartsImpl( + int num_outputs, + const 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]), @@ -251,15 +257,15 @@ typename ParameterDims, typename Functor, typename T> -inline bool AutoDifferentiate(const Functor& functor, - T const* const* parameters, - int dynamic_num_outputs, - T* function_value, - T** jacobians) { +EIGEN_STRONG_INLINE bool AutoDifferentiate(const Functor& functor, + T const* const* parameters, + int dynamic_num_outputs, + T* function_value, + T** jacobians) { using JetT = Jet<T, ParameterDims::kNumParameters>; using Parameters = typename ParameterDims::Parameters; - if (kNumResiduals != DYNAMIC) { + if constexpr (kNumResiduals != DYNAMIC) { DCHECK_EQ(kNumResiduals, dynamic_num_outputs); } @@ -281,12 +287,10 @@ DCHECK_GT(num_outputs, 0); ArraySelector<JetT, kNumResiduals, CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK> - residuals_as_jets(num_outputs); - - for (int i = 0; i < num_outputs; ++i) { - residuals_as_jets[i].a = kImpossibleValue; - residuals_as_jets[i].v.setConstant(kImpossibleValue); - } + residuals_as_jets( + num_outputs, + JetT(T(kImpossibleValue), + decltype(JetT::v)::Constant(T(kImpossibleValue)))); using Offsets = ExclusiveScan<Parameters>; Make1stOrderPerturbationsImpl(
diff --git a/include/ceres/internal/parameter_dims.h b/include/ceres/internal/parameter_dims.h index 3024a5f..c6f0ab4 100644 --- a/include/ceres/internal/parameter_dims.h +++ b/include/ceres/internal/parameter_dims.h
@@ -36,6 +36,7 @@ #include <array> #include <utility> +#include "Eigen/Core" #include "ceres/internal/integer_sequence_algorithm.h" namespace ceres::internal { @@ -74,16 +75,16 @@ // If one has all parameters packed into a single array, this function unpacks // the parameters into an array of pointers. template <typename T> - static inline std::array<T*, kNumParameterBlocks> GetUnpackedParameters( - T* ptr) { + static constexpr EIGEN_STRONG_INLINE std::array<T*, kNumParameterBlocks> + GetUnpackedParameters(T* ptr) { using Offsets = ExclusiveScan<Parameters>; return GetUnpackedParameters(ptr, Offsets()); } private: template <typename T, int... Indices> - static inline std::array<T*, kNumParameterBlocks> GetUnpackedParameters( - T* ptr, std::integer_sequence<int, Indices...>) { + static constexpr EIGEN_STRONG_INLINE std::array<T*, kNumParameterBlocks> + GetUnpackedParameters(T* ptr, std::integer_sequence<int, Indices...>) { return std::array<T*, kNumParameterBlocks>{{ptr + Indices...}}; }
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h index 133ba3a..041b2f8 100644 --- a/include/ceres/internal/variadic_evaluate.h +++ b/include/ceres/internal/variadic_evaluate.h
@@ -35,14 +35,11 @@ // IWYU pragma: private -#include <algorithm> -#include <array> #include <cstddef> -#include <functional> -#include <tuple> #include <type_traits> #include <utility> +#include "Eigen/Core" #include "ceres/cost_function.h" #include "ceres/internal/parameter_dims.h" @@ -61,34 +58,39 @@ // 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 Functor, typename T, std::size_t... Is> +EIGEN_STRONG_INLINE bool VariadicEvaluateImpl(const Functor& functor, + T const* const* input, + T* output, + std::index_sequence<Is...>) { + static_assert( + std::is_invocable_v<const Functor&, + decltype((void(Is), std::declval<const T*>()))..., + T*>, + "The provided Functor's operator() signature does not match " + "the expected number of parameter blocks defined in " + "ParameterDims."); + return functor(input[Is]..., output); +} + template <typename ParameterDims, typename Functor, typename T> -inline bool VariadicEvaluate(const Functor& functor, - T const* const* input, - T* output) { +EIGEN_STRONG_INLINE bool VariadicEvaluate(const Functor& functor, + T const* const* input, + T* output) { if constexpr (std::is_base_of_v<CostFunction, Functor>) { return functor.Evaluate(input, output, nullptr); + } else if constexpr (ParameterDims::kIsDynamic) { + return functor(input, output); } else { - if constexpr (ParameterDims::kIsDynamic) { - return functor(input, output); - } else { - constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks; - static_assert(kNumParameterBlocks > 0, - "Invalid number of parameter blocks. At least one " - "parameter block must be specified."); - - std::array<const T*, kNumParameterBlocks> parameter_blocks; - std::copy_n(input, kNumParameterBlocks, parameter_blocks.begin()); - return std::apply( - [&functor, output](auto const*... blocks) { - static_assert( - std::is_invocable_v<const Functor&, decltype(blocks)..., T*>, - "The provided Functor's operator() signature does not match " - "the expected number of parameter blocks defined in " - "ParameterDims."); - return std::invoke(functor, blocks..., output); - }, - parameter_blocks); - } + constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks; + static_assert(kNumParameterBlocks > 0, + "Invalid number of parameter blocks. At least one " + "parameter block must be specified."); + return VariadicEvaluateImpl( + functor, + input, + output, + std::make_index_sequence<kNumParameterBlocks>{}); } }
diff --git a/internal/ceres/array_selector_test.cc b/internal/ceres/array_selector_test.cc index 0b6c893..434f0b3 100644 --- a/internal/ceres/array_selector_test.cc +++ b/internal/ceres/array_selector_test.cc
@@ -33,7 +33,6 @@ #include <array> #include <type_traits> -#include <vector> #include "absl/container/fixed_array.h" #include "ceres/types.h" @@ -41,9 +40,7 @@ namespace ceres::internal { -// This test only checks, if the correct array implementations are selected. The -// test for FixedArray is in fixed_array_test.cc. Tests for std::array and -// std::vector are not included in ceres. +// This test only checks if the correct array implementations are selected. TEST(ArraySelector, FixedArray) { ArraySelector<int, DYNAMIC, 20> array1(10); static_assert( @@ -66,14 +63,33 @@ EXPECT_EQ(array2.size(), 20); } -TEST(ArraySelector, Vector) { +TEST(ArraySelector, HeapFixedArray) { ArraySelector<int, 20, 10> array1(20); - static_assert(std::is_base_of<std::vector<int>, decltype(array1)>::value); + static_assert( + std::is_base_of<absl::FixedArray<int, 0>, decltype(array1)>::value); EXPECT_EQ(array1.size(), 20); ArraySelector<int, 1, 0> array2(1); - static_assert(std::is_base_of<std::vector<int>, decltype(array2)>::value); + static_assert( + std::is_base_of<absl::FixedArray<int, 0>, decltype(array2)>::value); EXPECT_EQ(array2.size(), 1); } +TEST(ArraySelector, FillConstructor) { + ArraySelector<int, DYNAMIC, 20> array1(10, 42); + for (int v : array1) { + EXPECT_EQ(v, 42); + } + + ArraySelector<int, 10, 20> array2(10, -7); + for (int v : array2) { + EXPECT_EQ(v, -7); + } + + ArraySelector<int, 20, 10> array3(20, 99); + for (int v : array3) { + EXPECT_EQ(v, 99); + } +} + } // namespace ceres::internal