[modernize] Modernize ProductManifold using fold expressions Modernize ProductManifold using C++17 fold expressions and std::index_sequence to replace recursive implementations of Plus, Minus, and their Jacobians. This simplifies the code, removes the need for auxiliary recursive ...Impl methods, and improves index handling by using std::get<I> instead of pointer arithmetic. Change-Id: I28498216d8daae7485c382140613fd85519fa42c
diff --git a/include/ceres/product_manifold.h b/include/ceres/product_manifold.h index 9fc836e..37c3b0b 100644 --- a/include/ceres/product_manifold.h +++ b/include/ceres/product_manifold.h
@@ -103,15 +103,13 @@ bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { - return PlusImpl( - x, delta, x_plus_delta, std::make_index_sequence<kNumManifolds>{}); + return PlusImpl(x, delta, x_plus_delta, std::make_index_sequence<kNumManifolds>{}); } bool Minus(const double* y, const double* x, double* y_minus_x) const override { - return MinusImpl( - y, x, y_minus_x, std::make_index_sequence<kNumManifolds>{}); + return MinusImpl(y, x, y_minus_x, std::make_index_sequence<kNumManifolds>{}); } bool PlusJacobian(const double* x, double* jacobian_ptr) const override { @@ -119,8 +117,7 @@ jacobian.setZero(); absl::FixedArray<double> buffer(buffer_size_); - return PlusJacobianImpl( - x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{}); + return PlusJacobianImpl(x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{}); } bool MinusJacobian(const double* x, double* jacobian_ptr) const override { @@ -128,8 +125,7 @@ jacobian.setZero(); absl::FixedArray<double> buffer(buffer_size_); - return MinusJacobianImpl( - x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{}); + return MinusJacobianImpl(x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{}); } private: @@ -152,116 +148,77 @@ tangent_size_{ std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)} {} - template <std::size_t Index0, std::size_t... Indices> + template <std::size_t... Indices> bool PlusImpl(const double* x, const double* delta, double* x_plus_delta, - std::index_sequence<Index0, Indices...>) const { - if (!Dereference(std::get<Index0>(manifolds_)) - .Plus(x + ambient_offsets_[Index0], - delta + tangent_offsets_[Index0], - x_plus_delta + ambient_offsets_[Index0])) { - return false; - } - - return PlusImpl(x, delta, x_plus_delta, std::index_sequence<Indices...>{}); + std::index_sequence<Indices...>) const { + return (Dereference(std::get<Indices>(manifolds_)) + .Plus(x + ambient_offsets_[Indices], + delta + tangent_offsets_[Indices], + x_plus_delta + ambient_offsets_[Indices]) && + ...); } - static constexpr bool PlusImpl(const double* /*x*/, - const double* /*delta*/, - double* /*x_plus_delta*/, - std::index_sequence<>) noexcept { - return true; - } - - template <std::size_t Index0, std::size_t... Indices> + template <std::size_t... Indices> bool MinusImpl(const double* y, const double* x, double* y_minus_x, - std::index_sequence<Index0, Indices...>) const { - if (!Dereference(std::get<Index0>(manifolds_)) - .Minus(y + ambient_offsets_[Index0], - x + ambient_offsets_[Index0], - y_minus_x + tangent_offsets_[Index0])) { - return false; - } - - return MinusImpl(y, x, y_minus_x, std::index_sequence<Indices...>{}); + std::index_sequence<Indices...>) const { + return (Dereference(std::get<Indices>(manifolds_)) + .Minus(y + ambient_offsets_[Indices], + x + ambient_offsets_[Indices], + y_minus_x + tangent_offsets_[Indices]) && + ...); } - static constexpr bool MinusImpl(const double* /*y*/, - const double* /*x*/, - double* /*y_minus_x*/, - std::index_sequence<>) noexcept { - return true; - } - - template <std::size_t Index0, std::size_t... Indices> + template <std::size_t... Indices> bool PlusJacobianImpl(const double* x, MatrixRef& jacobian, absl::FixedArray<double>& buffer, - std::index_sequence<Index0, Indices...>) const { - if (!Dereference(std::get<Index0>(manifolds_)) - .PlusJacobian(x + ambient_offsets_[Index0], buffer.data())) { - return false; - } - - jacobian.block(ambient_offsets_[Index0], - tangent_offsets_[Index0], - ambient_sizes_[Index0], - tangent_sizes_[Index0]) = - MatrixRef( - buffer.data(), ambient_sizes_[Index0], tangent_sizes_[Index0]); - - return PlusJacobianImpl( - x, jacobian, buffer, std::index_sequence<Indices...>{}); + std::index_sequence<Indices...>) const { + auto apply_jacobian = [&](auto index_constant) { + constexpr std::size_t i = index_constant(); + if (!Dereference(std::get<i>(manifolds_)) + .PlusJacobian(x + ambient_offsets_[i], buffer.data())) { + return false; + } + jacobian.block(ambient_offsets_[i], + tangent_offsets_[i], + ambient_sizes_[i], + tangent_sizes_[i]) = + MatrixRef(buffer.data(), ambient_sizes_[i], tangent_sizes_[i]); + return true; + }; + return (apply_jacobian(std::integral_constant<std::size_t, Indices>{}) && ...); } - static constexpr bool PlusJacobianImpl(const double* /*x*/, - MatrixRef& /*jacobian*/, - absl::FixedArray<double>& /*buffer*/, - std::index_sequence<>) noexcept { - return true; - } - - template <std::size_t Index0, std::size_t... Indices> + template <std::size_t... Indices> bool MinusJacobianImpl(const double* x, MatrixRef& jacobian, absl::FixedArray<double>& buffer, - std::index_sequence<Index0, Indices...>) const { - if (!Dereference(std::get<Index0>(manifolds_)) - .MinusJacobian(x + ambient_offsets_[Index0], buffer.data())) { - return false; - } - - jacobian.block(tangent_offsets_[Index0], - ambient_offsets_[Index0], - tangent_sizes_[Index0], - ambient_sizes_[Index0]) = - MatrixRef( - buffer.data(), tangent_sizes_[Index0], ambient_sizes_[Index0]); - - return MinusJacobianImpl( - x, jacobian, buffer, std::index_sequence<Indices...>{}); - } - - static constexpr bool MinusJacobianImpl(const double* /*x*/, - MatrixRef& /*jacobian*/, - absl::FixedArray<double>& /*buffer*/, - std::index_sequence<>) noexcept { - return true; + std::index_sequence<Indices...>) const { + auto apply_jacobian = [&](auto index_constant) { + constexpr std::size_t i = index_constant(); + if (!Dereference(std::get<i>(manifolds_)) + .MinusJacobian(x + ambient_offsets_[i], buffer.data())) { + return false; + } + jacobian.block(tangent_offsets_[i], + ambient_offsets_[i], + tangent_sizes_[i], + ambient_sizes_[i]) = + MatrixRef(buffer.data(), tangent_sizes_[i], ambient_sizes_[i]); + return true; + }; + return (apply_jacobian(std::integral_constant<std::size_t, Indices>{}) && + ...); } template <typename T, std::size_t N> static std::array<T, N> ExclusiveScan(const std::array<T, N>& values) { std::array<T, N> result; - // TODO Replace with std::exclusive_scan once all platforms have full C++17 - // STL support. - T init = 0; - for (std::size_t i = 0; i != N; ++i) { - result[i] = init; - init += values[i]; - } + std::exclusive_scan(values.begin(), values.end(), result.begin(), T(0)); return result; }