Allow ProductManifold default construction In many cases, manifolds stored in ProductManifold have a default constructor which can simplify ProductManifold initialization even further. Allow default construction of ProductManifold in this case. Change-Id: I29b2612870c02232556688019a77049709684a55
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 5bc5382..51afcec 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst
@@ -1528,7 +1528,7 @@ In cases, where a parameter block is the Cartesian product of a number of manifolds and you have the manifold of the individual parameter blocks available, :class:`ProductManifold` can be used to -construct a :class:`Manifold` of the cartesian product. +construct a :class:`Manifold` of the Cartesian product. For the case of the rigid transformation, where say you have a parameter block of size 7, where the first four entries represent the @@ -1537,8 +1537,16 @@ .. code-block:: c++ - ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3{QuaternionManifold{}, EuclideanManifold<3>{}}; + ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3; +Manifolds can be copied and moved to :class:`ProductManifold`: + +.. code-block:: c++ + + SubsetManifold manifold1(5, {2}); + SubsetManifold manifold2(3, {0, 1}); + ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1, + manifold2); In C++17, the template parameters can be left out as they are automatically deduced making the initialization much simpler: @@ -2153,7 +2161,7 @@ In cases, where a parameter block is the Cartesian product of a number of manifolds and you have the local parameterization of the individual manifolds available, :class:`ProductParameterization` can be used to -construct a local parameterization of the cartesian product. For the +construct a local parameterization of the Cartesian product. For the case of the rigid transformation, where say you have a parameter block of size 7, where the first four entries represent the rotation as a quaternion, a local parameterization can be constructed as
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc index d42a0d7..19c9c4b 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc
@@ -306,8 +306,7 @@ if (CERES_GET_FLAG(FLAGS_use_quaternions) && CERES_GET_FLAG(FLAGS_use_manifolds)) { Manifold* camera_manifold = - new ProductManifold<QuaternionManifold, EuclideanManifold<6>>{ - QuaternionManifold{}, EuclideanManifold<6>{}}; + new ProductManifold<QuaternionManifold, EuclideanManifold<6>>{}; for (int i = 0; i < bal_problem->num_cameras(); ++i) { problem->SetManifold(cameras + camera_block_size * i, camera_manifold); }
diff --git a/include/ceres/product_manifold.h b/include/ceres/product_manifold.h index bd6131f..b7ebe4d 100644 --- a/include/ceres/product_manifold.h +++ b/include/ceres/product_manifold.h
@@ -55,41 +55,40 @@ // // Example usage: // -// ProductManifold<QuaternionManifold, EuclideanManifold<3>> -// se3(QuaternionManifold(), EuclideanManifold<3>()); +// ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3; // // is the manifold for a rigid transformation, where the rotation is // represented using a quaternion. // +// Manifolds can be copied and moved to ProductManifold: +// +// SubsetManifold manifold1(5, {2}); +// SubsetManifold manifold2(3, {0, 1}); +// ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1, +// manifold2); +// // In C++17, the template parameters can be left out as they are automatically // deduced making the initialization much simpler: // // ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}}; // -// The manifold implementations must be either copyable or moveable to be usable -// in a ProductManifold. -template <typename... Manifolds> +// The manifold implementations must be either default constructible, copyable +// or moveable to be usable in a ProductManifold. +template <typename Manifold0, typename Manifold1, typename... ManifoldN> class ProductManifold final : public Manifold { - struct InitTag {}; - public: - template < - typename Manifold0, - typename Manifold1, - typename... ManifoldN, - std::enable_if_t<std::is_constructible<std::tuple<Manifolds...>, - Manifold0, - Manifold1, - ManifoldN...>::value>* = nullptr> - ProductManifold(Manifold0&& manifold0, - Manifold1&& manifold1, - ManifoldN&&... manifolds) - : ProductManifold{ - InitTag{}, // Use InitTag to disambiguate public/private - // constructors, which both use variadic arguments - std::forward<Manifold0>(manifold0), - std::forward<Manifold1>(manifold1), - std::forward<ManifoldN>(manifolds)...} {} + // ProductManifold constructor perfect forwards arguments to store manifolds. + // + // Either use default construction or if you need to copy or move-construct a + // manifold instance, you need to pass an instance as an argument for all + // types given as class template parameters. + template <typename... Args, + std::enable_if_t<std::is_constructible< + std::tuple<Manifold0, Manifold1, ManifoldN...>, + Args...>::value>* = nullptr> + explicit ProductManifold(Args&&... manifolds) + : ProductManifold{std::make_index_sequence<kNumManifolds>{}, + std::forward<Args>(manifolds)...} {} int AmbientSize() const override { return ambient_size_; } int TangentSize() const override { return tangent_size_; } @@ -127,27 +126,22 @@ } private: - static constexpr std::size_t kNumManifolds = sizeof...(Manifolds); + static constexpr std::size_t kNumManifolds = 2 + sizeof...(ManifoldN); - // In the public constructor, we enforce at least two parameters. Then, the - // public constructor delegates the arguments to this private constructor to - // be able to construct-initialize the manifolds tuple. The delegation is - // necessary to avoid requiring the manifolds to be default constructible. - // InitTag is used as a token to disambiguate between both variadic parameter - // constructors. - template <typename... Args> - explicit ProductManifold(InitTag, Args&&... manifolds) - : buffer_size_{(std::max)( - {(manifolds.TangentSize() * manifolds.AmbientSize())...})}, - ambient_sizes_{manifolds.AmbientSize()...}, - tangent_sizes_{manifolds.TangentSize()...}, + template <std::size_t... Indices, typename... Args> + explicit ProductManifold(std::index_sequence<Indices...>, Args&&... manifolds) + : manifolds_{std::forward<Args>(manifolds)...}, + buffer_size_{ + (std::max)({(std::get<Indices>(manifolds_).TangentSize() * + std::get<Indices>(manifolds_).AmbientSize())...})}, + ambient_sizes_{std::get<Indices>(manifolds_).AmbientSize()...}, + tangent_sizes_{std::get<Indices>(manifolds_).TangentSize()...}, ambient_offsets_{ExclusiveScan(ambient_sizes_)}, tangent_offsets_{ExclusiveScan(tangent_sizes_)}, ambient_size_{ std::accumulate(ambient_sizes_.begin(), ambient_sizes_.end(), 0)}, tangent_size_{ - std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)}, - manifolds_{std::forward<Args>(manifolds)...} {} + std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)} {} template <std::size_t Index0, std::size_t... Indices> bool PlusImpl(const double* x, @@ -265,6 +259,7 @@ return result; } + std::tuple<Manifold0, Manifold1, ManifoldN...> manifolds_; int buffer_size_; std::array<int, kNumManifolds> ambient_sizes_; std::array<int, kNumManifolds> tangent_sizes_; @@ -272,7 +267,6 @@ std::array<int, kNumManifolds> tangent_offsets_; int ambient_size_; int tangent_size_; - std::tuple<Manifolds...> manifolds_; }; #ifdef CERES_HAS_CPP17
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 60f98d2..6210c7a 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc
@@ -428,6 +428,57 @@ EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); } +struct NonDefaultConstructibleManifold : ceres::Manifold { + NonDefaultConstructibleManifold(int, int) {} + int AmbientSize() const override { return 4; } + int TangentSize() const override { return 3; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { + return true; + } + + bool PlusJacobian(const double* x, double* jacobian) const override { + return true; + } + + bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const override { + return true; + } + + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override { + return true; + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + return true; + } +}; + +TEST(ProductManifold, NonDefaultConstructible) { + ProductManifold<NonDefaultConstructibleManifold, QuaternionManifold> + manifold1{NonDefaultConstructibleManifold{1, 2}, QuaternionManifold{}}; + ProductManifold<QuaternionManifold, NonDefaultConstructibleManifold> + manifold2{QuaternionManifold{}, NonDefaultConstructibleManifold{1, 2}}; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + +TEST(ProductManifold, DefaultConstructible) { + ProductManifold<EuclideanManifold<3>, SphereManifold<4>> manifold1; + ProductManifold<SphereManifold<4>, EuclideanManifold<3>> manifold2; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + TEST(QuaternionManifold, PlusPiBy2) { QuaternionManifold manifold; Vector x = Vector::Zero(4);