diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 4f754c932..5bc5382ac 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst @@ -1537,7 +1537,15 @@ manifold can be constructed as: .. code-block:: c++ - ProductManifold se3(new QuaternionManifold, new EuclideanManifold<3>); + ProductManifold> se3{QuaternionManifold{}, EuclideanManifold<3>{}}; + + +In C++17, the template parameters can be left out as they are automatically +deduced making the initialization much simpler: + +.. code-block:: c++ + + ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}}; :class:`QuaternionManifold` diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc index b3590e936..d42a0d716 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc @@ -306,7 +306,8 @@ 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(new QuaternionManifold, new 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/ceres.h b/include/ceres/ceres.h index b67b5e66e..c32477d42 100644 --- a/include/ceres/ceres.h +++ b/include/ceres/ceres.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -64,6 +64,7 @@ #include "ceres/numeric_diff_options.h" #include "ceres/ordered_groups.h" #include "ceres/problem.h" +#include "ceres/product_manifold.h" #include "ceres/sized_cost_function.h" #include "ceres/solver.h" #include "ceres/sphere_manifold.h" diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h index d50de7ff1..24eac2325 100644 --- a/include/ceres/manifold.h +++ b/include/ceres/manifold.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2021 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -35,6 +35,7 @@ #include #include #include +#include #include #include "ceres/internal/disable_warnings.h" @@ -337,72 +338,6 @@ class CERES_EXPORT SubsetManifold final : public Manifold { std::vector constancy_mask_; }; -// Construct a manifold by taking the Cartesian product of a number of other -// manifolds. This is useful, when a parameter block is the cartesian product -// of two or more manifolds. For example the parameters of a camera consist of -// a rotation and a translation, i.e., SO(3) x R^3. -// -// Example usage: -// -// ProductParameterization se3(new Quaternion(), new EuclideanManifold(3)); -// -// is the manifold for a rigid transformation, where the rotation is -// represented using a quaternion. -class CERES_EXPORT ProductManifold final : public Manifold { - public: - ProductManifold(const ProductManifold&) = delete; - ProductManifold& operator=(const ProductManifold&) = delete; - // NOTE: Do not remove the trivial destructor as this will cause linker - // errors in MSVC builds. - ~ProductManifold() override; - - // NOTE: The constructor takes ownership of the input - // manifolds. - // - template - explicit ProductManifold(Manifolds*... manifolds) - : manifolds_(sizeof...(Manifolds)) { - constexpr int kNumManifolds = sizeof...(Manifolds); - static_assert(kNumManifolds >= 2, - "At least two manifolds must be specified."); - - using ManifoldPtr = std::unique_ptr; - - // Wrap all raw pointers into std::unique_ptr for exception safety. - std::array manifolds_array{ - ManifoldPtr(manifolds)...}; - - // Initialize internal state. - for (int i = 0; i < kNumManifolds; ++i) { - ManifoldPtr& manifold = manifolds_[i]; - manifold = std::move(manifolds_array[i]); - - buffer_size_ = (std::max)( - buffer_size_, manifold->TangentSize() * manifold->AmbientSize()); - ambient_size_ += manifold->AmbientSize(); - tangent_size_ += manifold->TangentSize(); - } - } - - int AmbientSize() const override; - int TangentSize() const override; - - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool PlusJacobian(const double* x, double* jacobian) const override; - bool Minus(const double* y, - const double* x, - double* y_minus_x) const override; - bool MinusJacobian(const double* x, double* jacobian) const override; - - private: - std::vector> manifolds_; - int ambient_size_ = 0; - int tangent_size_ = 0; - int buffer_size_ = 0; -}; - // Implements the manifold for a Hamilton quaternion as defined in // https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as // unit norm 4-vectors, i.e. diff --git a/include/ceres/product_manifold.h b/include/ceres/product_manifold.h new file mode 100644 index 000000000..bd6131fbe --- /dev/null +++ b/include/ceres/product_manifold.h @@ -0,0 +1,292 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// sergiu.deitsch@gmail.com (Sergiu Deitsch) +// + +#ifndef CERES_PUBLIC_PRODUCT_MANIFOLD_H_ +#define CERES_PUBLIC_PRODUCT_MANIFOLD_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/port.h" +#include "ceres/manifold.h" + +namespace ceres { + +// Construct a manifold by taking the Cartesian product of a number of other +// manifolds. This is useful, when a parameter block is the Cartesian product +// of two or more manifolds. For example the parameters of a camera consist of +// a rotation and a translation, i.e., SO(3) x R^3. +// +// Example usage: +// +// ProductManifold> +// se3(QuaternionManifold(), EuclideanManifold<3>()); +// +// is the manifold for a rigid transformation, where the rotation is +// represented using a quaternion. +// +// 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 +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)...} {} + + int AmbientSize() const override { return ambient_size_; } + int TangentSize() const override { return tangent_size_; } + + 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{}); + } + + 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{}); + } + + bool PlusJacobian(const double* x, double* jacobian_ptr) const override { + MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize()); + jacobian.setZero(); + internal::FixedArray buffer(buffer_size_); + + return PlusJacobianImpl( + x, jacobian, buffer, std::make_index_sequence{}); + } + + bool MinusJacobian(const double* x, double* jacobian_ptr) const override { + MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize()); + jacobian.setZero(); + internal::FixedArray buffer(buffer_size_); + + return MinusJacobianImpl( + x, jacobian, buffer, std::make_index_sequence{}); + } + + private: + static constexpr std::size_t kNumManifolds = sizeof...(Manifolds); + + // 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()...}, + 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)...} {} + + template + bool PlusImpl(const double* x, + const double* delta, + double* x_plus_delta, + std::index_sequence) const { + if (!std::get(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{}); + } + + static constexpr bool PlusImpl(const double* /*x*/, + const double* /*delta*/, + double* /*x_plus_delta*/, + std::index_sequence<>) noexcept { + return true; + } + + template + bool MinusImpl(const double* y, + const double* x, + double* y_minus_x, + std::index_sequence) const { + if (!std::get(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{}); + } + + static constexpr bool MinusImpl(const double* /*y*/, + const double* /*x*/, + double* /*y_minus_x*/, + std::index_sequence<>) noexcept { + return true; + } + + template + bool PlusJacobianImpl(const double* x, + MatrixRef& jacobian, + internal::FixedArray& buffer, + std::index_sequence) const { + if (!std::get(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{}); + } + + static constexpr bool PlusJacobianImpl( + const double* /*x*/, + MatrixRef& /*jacobian*/, + internal::FixedArray& /*buffer*/, + std::index_sequence<>) noexcept { + return true; + } + + template + bool MinusJacobianImpl(const double* x, + MatrixRef& jacobian, + internal::FixedArray& buffer, + std::index_sequence) const { + if (!std::get(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{}); + } + + static constexpr bool MinusJacobianImpl( + const double* /*x*/, + MatrixRef& /*jacobian*/, + internal::FixedArray& /*buffer*/, + std::index_sequence<>) noexcept { + return true; + } + + template + static std::array ExclusiveScan(const std::array& values) { + std::array result; + T init = 0; + + // TODO Replace by std::exclusive_scan once C++17 is available + for (std::size_t i = 0; i != N; ++i) { + result[i] = init; + init += values[i]; + } + + return result; + } + + int buffer_size_; + std::array ambient_sizes_; + std::array tangent_sizes_; + std::array ambient_offsets_; + std::array tangent_offsets_; + int ambient_size_; + int tangent_size_; + std::tuple manifolds_; +}; + +#ifdef CERES_HAS_CPP17 +// C++17 deduction guide that allows the user to avoid explicitly specifying +// the template parameters of ProductManifold. The class can instead be +// instantiated as follows: +// +// ProductManifold manifold{QuaternionManifold{}, EuclideanManifold<3>{}}; +// +template +ProductManifold(Manifold0&&, Manifold1&&, Manifolds&&...) + -> ProductManifold; +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_PRODUCT_MANIFOLD_H_ diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc index b2ff9538c..f412a793f 100644 --- a/internal/ceres/manifold.cc +++ b/internal/ceres/manifold.cc @@ -267,112 +267,6 @@ bool SubsetManifold::MinusJacobian(const double* x, return true; } -// While the trivial destructor is not necessary, MSVC (1930) complains in -// shared library builds by issuing a linker error: -// -// bundle_adjuster.obj : error LNK2019: unresolved external symbol -// "__declspec(dllimport) const ceres::ProductManifold::`vftable'" -// (__imp_??_7ProductManifold@ceres@@6B@) referenced in function "public: -// __cdecl ceres::ProductManifold::ProductManifold(class -// ceres::QuaternionManifold *,class ceres::EuclideanManifold *)" -// (??$?0VQuaternionManifold@ceres@@VEuclideanManifold@1@@ProductManifold@ceres@@QEAA@PEAVQuaternionManifold@1@PEAVEuclideanManifold@1@@Z) -// -// Providing a trivial implementation here resolves the issue. -ProductManifold::~ProductManifold() = default; - -int ProductManifold::AmbientSize() const { return ambient_size_; } - -int ProductManifold::TangentSize() const { return tangent_size_; } - -bool ProductManifold::Plus(const double* x, - const double* delta, - double* x_plus_delta) const { - int ambient_cursor = 0; - int tangent_cursor = 0; - for (const auto& m : manifolds_) { - if (!m->Plus(x + ambient_cursor, - delta + tangent_cursor, - x_plus_delta + ambient_cursor)) { - return false; - } - tangent_cursor += m->TangentSize(); - ambient_cursor += m->AmbientSize(); - } - - return true; -} - -bool ProductManifold::PlusJacobian(const double* x, - double* jacobian_ptr) const { - MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize()); - jacobian.setZero(); - internal::FixedArray buffer(buffer_size_); - - int ambient_cursor = 0; - int tangent_cursor = 0; - for (const auto& m : manifolds_) { - const int ambient_size = m->AmbientSize(); - const int tangent_size = m->TangentSize(); - - if (!m->PlusJacobian(x + ambient_cursor, buffer.data())) { - return false; - } - - jacobian.block(ambient_cursor, tangent_cursor, ambient_size, tangent_size) = - MatrixRef(buffer.data(), ambient_size, tangent_size); - - ambient_cursor += ambient_size; - tangent_cursor += tangent_size; - } - - return true; -} - -bool ProductManifold::Minus(const double* y, - const double* x, - double* y_minus_x) const { - int ambient_cursor = 0; - int tangent_cursor = 0; - for (const auto& m : manifolds_) { - if (!m->Minus(y + ambient_cursor, - x + ambient_cursor, - y_minus_x + tangent_cursor)) { - return false; - } - tangent_cursor += m->TangentSize(); - ambient_cursor += m->AmbientSize(); - } - - return true; -} - -bool ProductManifold::MinusJacobian(const double* x, - double* jacobian_ptr) const { - MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize()); - jacobian.setZero(); - internal::FixedArray buffer(buffer_size_); - - int ambient_cursor = 0; - int tangent_cursor = 0; - for (const auto& m : manifolds_) { - const int ambient_size = m->AmbientSize(); - const int tangent_size = m->TangentSize(); - - if (!m->MinusJacobian(x + ambient_cursor, buffer.data())) { - return false; - } - - jacobian.block(tangent_cursor, ambient_cursor, tangent_size, ambient_size) = - MatrixRef(buffer.data(), tangent_size, ambient_size); - - ambient_cursor += ambient_size; - tangent_cursor += tangent_size; - } - - return true; -} - bool QuaternionManifold::Plus(const double* x, const double* delta, double* x_plus_delta) const { diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index b697d0278..60f98d2ee 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2021 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -41,6 +41,7 @@ #include "ceres/line_manifold.h" #include "ceres/manifold_test_utils.h" #include "ceres/numeric_diff_options.h" +#include "ceres/product_manifold.h" #include "ceres/rotation.h" #include "ceres/sphere_manifold.h" #include "ceres/types.h" @@ -158,54 +159,64 @@ TEST(SubsetManifold, NormalFunctionTest) { } TEST(ProductManifold, Size2) { - Manifold* manifold1 = new SubsetManifold(5, {2}); - Manifold* manifold2 = new SubsetManifold(3, {0, 1}); - ProductManifold manifold(manifold1, manifold2); + SubsetManifold manifold1(5, {2}); + SubsetManifold manifold2(3, {0, 1}); + ProductManifold manifold(manifold1, + manifold2); EXPECT_EQ(manifold.AmbientSize(), - manifold1->AmbientSize() + manifold2->AmbientSize()); + manifold1.AmbientSize() + manifold2.AmbientSize()); EXPECT_EQ(manifold.TangentSize(), - manifold1->TangentSize() + manifold2->TangentSize()); + manifold1.TangentSize() + manifold2.TangentSize()); } TEST(ProductManifold, Size3) { - Manifold* manifold1 = new SubsetManifold(5, {2}); - Manifold* manifold2 = new SubsetManifold(3, {0, 1}); - Manifold* manifold3 = new SubsetManifold(4, {1}); + SubsetManifold manifold1(5, {2}); + SubsetManifold manifold2(3, {0, 1}); + SubsetManifold manifold3(4, {1}); - ProductManifold manifold(manifold1, manifold2, manifold3); + ProductManifold manifold( + manifold1, manifold2, manifold3); EXPECT_EQ(manifold.AmbientSize(), - manifold1->AmbientSize() + manifold2->AmbientSize() + - manifold3->AmbientSize()); + manifold1.AmbientSize() + manifold2.AmbientSize() + + manifold3.AmbientSize()); EXPECT_EQ(manifold.TangentSize(), - manifold1->TangentSize() + manifold2->TangentSize() + - manifold3->TangentSize()); + manifold1.TangentSize() + manifold2.TangentSize() + + manifold3.TangentSize()); } TEST(ProductManifold, Size4) { - Manifold* manifold1 = new SubsetManifold(5, {2}); - Manifold* manifold2 = new SubsetManifold(3, {0, 1}); - Manifold* manifold3 = new SubsetManifold(4, {1}); - Manifold* manifold4 = new SubsetManifold(2, {0}); + SubsetManifold manifold1(5, {2}); + SubsetManifold manifold2(3, {0, 1}); + SubsetManifold manifold3(4, {1}); + SubsetManifold manifold4(2, {0}); - ProductManifold manifold(manifold1, manifold2, manifold3, manifold4); + ProductManifold + manifold(manifold1, manifold2, manifold3, manifold4); EXPECT_EQ(manifold.AmbientSize(), - manifold1->AmbientSize() + manifold2->AmbientSize() + - manifold3->AmbientSize() + manifold4->AmbientSize()); + manifold1.AmbientSize() + manifold2.AmbientSize() + + manifold3.AmbientSize() + manifold4.AmbientSize()); EXPECT_EQ(manifold.TangentSize(), - manifold1->TangentSize() + manifold2->TangentSize() + - manifold3->TangentSize() + manifold4->TangentSize()); + manifold1.TangentSize() + manifold2.TangentSize() + + manifold3.TangentSize() + manifold4.TangentSize()); } TEST(ProductManifold, NormalFunctionTest) { - Manifold* manifold1 = new SubsetManifold(5, {2}); - Manifold* manifold2 = new SubsetManifold(3, {0, 1}); - Manifold* manifold3 = new SubsetManifold(4, {1}); - Manifold* manifold4 = new SubsetManifold(2, {0}); + SubsetManifold manifold1(5, {2}); + SubsetManifold manifold2(3, {0, 1}); + SubsetManifold manifold3(4, {1}); + SubsetManifold manifold4(2, {0}); - ProductManifold manifold(manifold1, manifold2, manifold3, manifold4); + ProductManifold + manifold(manifold1, manifold2, manifold3, manifold4); for (int trial = 0; trial < kNumTrials; ++trial) { const Vector x = Vector::Random(manifold.AmbientSize()); @@ -218,29 +229,29 @@ TEST(ProductManifold, NormalFunctionTest) { int ambient_cursor = 0; int tangent_cursor = 0; - EXPECT_TRUE(manifold1->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold1->AmbientSize(); - tangent_cursor += manifold1->TangentSize(); + EXPECT_TRUE(manifold1.Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold1.AmbientSize(); + tangent_cursor += manifold1.TangentSize(); - EXPECT_TRUE(manifold2->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold2->AmbientSize(); - tangent_cursor += manifold2->TangentSize(); + EXPECT_TRUE(manifold2.Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold2.AmbientSize(); + tangent_cursor += manifold2.TangentSize(); - EXPECT_TRUE(manifold3->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold3->AmbientSize(); - tangent_cursor += manifold3->TangentSize(); + EXPECT_TRUE(manifold3.Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold3.AmbientSize(); + tangent_cursor += manifold3.TangentSize(); - EXPECT_TRUE(manifold4->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold4->AmbientSize(); - tangent_cursor += manifold4->TangentSize(); + EXPECT_TRUE(manifold4.Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold4.AmbientSize(); + tangent_cursor += manifold4.TangentSize(); for (int i = 0; i < x.size(); ++i) { EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]); @@ -252,9 +263,10 @@ TEST(ProductManifold, NormalFunctionTest) { } TEST(ProductManifold, ZeroTangentSizeAndEuclidean) { - Manifold* subset_manifold = new SubsetManifold(1, {0}); - Manifold* euclidean_manifold = new EuclideanManifold<2>; - ProductManifold manifold(subset_manifold, euclidean_manifold); + SubsetManifold subset_manifold(1, {0}); + EuclideanManifold<2> euclidean_manifold; + ProductManifold> manifold( + subset_manifold, euclidean_manifold); EXPECT_EQ(manifold.AmbientSize(), 3); EXPECT_EQ(manifold.TangentSize(), 2); @@ -276,9 +288,10 @@ TEST(ProductManifold, ZeroTangentSizeAndEuclidean) { } TEST(ProductManifold, EuclideanAndZeroTangentSize) { - Manifold* subset_manifold = new SubsetManifold(1, {0}); - Manifold* euclidean_manifold = new EuclideanManifold<2>; - ProductManifold manifold(euclidean_manifold, subset_manifold); + SubsetManifold subset_manifold(1, {0}); + EuclideanManifold<2> euclidean_manifold; + ProductManifold, SubsetManifold> manifold( + euclidean_manifold, subset_manifold); EXPECT_EQ(manifold.AmbientSize(), 3); EXPECT_EQ(manifold.TangentSize(), 2); @@ -297,6 +310,124 @@ TEST(ProductManifold, EuclideanAndZeroTangentSize) { } } +struct CopyableManifold : ceres::Manifold { + CopyableManifold() = default; + CopyableManifold(const CopyableManifold&) = default; + // Do not care about copy-assignment + CopyableManifold& operator=(const CopyableManifold&) = delete; + // Not moveable + CopyableManifold(CopyableManifold&&) = delete; + CopyableManifold& operator=(CopyableManifold&&) = delete; + + int AmbientSize() const override { return 3; } + int TangentSize() const override { return 2; } + + 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; + } +}; + +struct MoveableManifold : ceres::Manifold { + MoveableManifold() = default; + MoveableManifold(MoveableManifold&&) = default; + // Do not care about move-assignment + MoveableManifold& operator=(MoveableManifold&&) = delete; + // Not copyable + MoveableManifold(const MoveableManifold&) = delete; + MoveableManifold& operator=(const MoveableManifold&) = delete; + + int AmbientSize() const override { return 3; } + int TangentSize() const override { return 2; } + + 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, CopyableOnly) { + ProductManifold> manifold1{ + CopyableManifold{}, EuclideanManifold<3>{}}; + + CopyableManifold inner2; + ProductManifold> manifold2{ + inner2, EuclideanManifold<3>{}}; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + +TEST(ProductManifold, MoveableOnly) { + ProductManifold> manifold1{ + MoveableManifold{}, EuclideanManifold<3>{}}; + + MoveableManifold inner2; + ProductManifold> manifold2{ + std::move(inner2), EuclideanManifold<3>{}}; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + +TEST(ProductManifold, CopyableOrMoveable) { + const CopyableManifold inner12{}; + ProductManifold manifold1{ + MoveableManifold{}, inner12}; + + MoveableManifold inner21; + CopyableManifold inner22; + ProductManifold manifold2{ + std::move(inner21), inner22}; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + TEST(QuaternionManifold, PlusPiBy2) { QuaternionManifold manifold; Vector x = Vector::Zero(4);