diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 5bc5382ac..51afcece1 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst @@ -1528,7 +1528,7 @@ coordinates that are held constant. 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 @@ manifold can be constructed as: .. code-block:: c++ - ProductManifold> se3{QuaternionManifold{}, EuclideanManifold<3>{}}; + ProductManifold> se3; +Manifolds can be copied and moved to :class:`ProductManifold`: + +.. code-block:: c++ + + SubsetManifold manifold1(5, {2}); + SubsetManifold manifold2(3, {0, 1}); + ProductManifold 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 @@ transformation? 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 d42a0d716..19c9c4b1f 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc @@ -306,8 +306,7 @@ void BuildProblem(BALProblem* bal_problem, Problem* problem) { if (CERES_GET_FLAG(FLAGS_use_quaternions) && CERES_GET_FLAG(FLAGS_use_manifolds)) { Manifold* camera_manifold = - new ProductManifold>{ - QuaternionManifold{}, EuclideanManifold<6>{}}; + new ProductManifold>{}; 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 bd6131fbe..b7ebe4d42 100644 --- a/include/ceres/product_manifold.h +++ b/include/ceres/product_manifold.h @@ -55,41 +55,40 @@ namespace ceres { // // Example usage: // -// ProductManifold> -// se3(QuaternionManifold(), EuclideanManifold<3>()); +// ProductManifold> 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 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 +// The manifold implementations must be either default constructible, copyable +// or moveable to be usable in a ProductManifold. +template class ProductManifold final : public Manifold { - struct InitTag {}; - public: - template < - typename Manifold0, - typename Manifold1, - typename... ManifoldN, - std::enable_if_t, - 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), - std::forward(manifold1), - std::forward(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 , + Args...>::value>* = nullptr> + explicit ProductManifold(Args&&... manifolds) + : ProductManifold{std::make_index_sequence{}, + std::forward(manifolds)...} {} int AmbientSize() const override { return ambient_size_; } int TangentSize() const override { return tangent_size_; } @@ -127,27 +126,22 @@ class ProductManifold final : public Manifold { } 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 - explicit ProductManifold(InitTag, Args&&... manifolds) - : buffer_size_{(std::max)( - {(manifolds.TangentSize() * manifolds.AmbientSize())...})}, - ambient_sizes_{manifolds.AmbientSize()...}, - tangent_sizes_{manifolds.TangentSize()...}, + template + explicit ProductManifold(std::index_sequence, Args&&... manifolds) + : manifolds_{std::forward(manifolds)...}, + buffer_size_{ + (std::max)({(std::get(manifolds_).TangentSize() * + std::get(manifolds_).AmbientSize())...})}, + ambient_sizes_{std::get(manifolds_).AmbientSize()...}, + tangent_sizes_{std::get(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(manifolds)...} {} + std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)} {} template bool PlusImpl(const double* x, @@ -265,6 +259,7 @@ class ProductManifold final : public Manifold { return result; } + std::tuple manifolds_; int buffer_size_; std::array ambient_sizes_; std::array tangent_sizes_; @@ -272,7 +267,6 @@ class ProductManifold final : public Manifold { std::array tangent_offsets_; int ambient_size_; int tangent_size_; - std::tuple manifolds_; }; #ifdef CERES_HAS_CPP17 diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 60f98d2ee..6210c7a74 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc @@ -428,6 +428,57 @@ TEST(ProductManifold, CopyableOrMoveable) { 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 + manifold1{NonDefaultConstructibleManifold{1, 2}, QuaternionManifold{}}; + ProductManifold + manifold2{QuaternionManifold{}, NonDefaultConstructibleManifold{1, 2}}; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + +TEST(ProductManifold, DefaultConstructible) { + ProductManifold, SphereManifold<4>> manifold1; + ProductManifold, 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);