Allow to store pointers in ProductManifold Change-Id: I32df7afab3a195efb0407b0d8f35dcd2d7cb95d2
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 51afcec..c6b7987 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst
@@ -1548,6 +1548,13 @@ ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1, manifold2); +In advanced use cases, manifolds can be dynamically allocated and passed as (smart) pointers: + +.. code-block:: c++ + + ProductManifold<std::unique_ptr<QuaternionManifold>, EuclideanManifold<3>> se3 + {std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}}; + In C++17, the template parameters can be left out as they are automatically deduced making the initialization much simpler:
diff --git a/include/ceres/product_manifold.h b/include/ceres/product_manifold.h index b7ebe4d..33f046d 100644 --- a/include/ceres/product_manifold.h +++ b/include/ceres/product_manifold.h
@@ -35,6 +35,7 @@ #include <algorithm> #include <array> +#include <cassert> #include <cstddef> #include <numeric> #include <tuple> @@ -67,6 +68,12 @@ // ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1, // manifold2); // +// In advanced use cases, manifolds can be dynamically allocated and passed as +// (smart) pointers: +// +// ProductManifold<std::unique_ptr<QuaternionManifold>, EuclideanManifold<3>> +// se3{std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}}; +// // In C++17, the template parameters can be left out as they are automatically // deduced making the initialization much simpler: // @@ -131,11 +138,13 @@ 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()...}, + buffer_size_{(std::max)( + {(Dereference(std::get<Indices>(manifolds_)).TangentSize() * + Dereference(std::get<Indices>(manifolds_)).AmbientSize())...})}, + ambient_sizes_{ + Dereference(std::get<Indices>(manifolds_)).AmbientSize()...}, + tangent_sizes_{ + Dereference(std::get<Indices>(manifolds_)).TangentSize()...}, ambient_offsets_{ExclusiveScan(ambient_sizes_)}, tangent_offsets_{ExclusiveScan(tangent_sizes_)}, ambient_size_{ @@ -148,7 +157,7 @@ const double* delta, double* x_plus_delta, std::index_sequence<Index0, Indices...>) const { - if (!std::get<Index0>(manifolds_) + if (!Dereference(std::get<Index0>(manifolds_)) .Plus(x + ambient_offsets_[Index0], delta + tangent_offsets_[Index0], x_plus_delta + ambient_offsets_[Index0])) { @@ -170,7 +179,7 @@ const double* x, double* y_minus_x, std::index_sequence<Index0, Indices...>) const { - if (!std::get<Index0>(manifolds_) + if (!Dereference(std::get<Index0>(manifolds_)) .Minus(y + ambient_offsets_[Index0], x + ambient_offsets_[Index0], y_minus_x + tangent_offsets_[Index0])) { @@ -192,7 +201,7 @@ MatrixRef& jacobian, internal::FixedArray<double>& buffer, std::index_sequence<Index0, Indices...>) const { - if (!std::get<Index0>(manifolds_) + if (!Dereference(std::get<Index0>(manifolds_)) .PlusJacobian(x + ambient_offsets_[Index0], buffer.data())) { return false; } @@ -221,7 +230,7 @@ MatrixRef& jacobian, internal::FixedArray<double>& buffer, std::index_sequence<Index0, Indices...>) const { - if (!std::get<Index0>(manifolds_) + if (!Dereference(std::get<Index0>(manifolds_)) .MinusJacobian(x + ambient_offsets_[Index0], buffer.data())) { return false; } @@ -259,6 +268,39 @@ return result; } + // TODO Replace by std::void_t once C++17 is available + template <typename... Types> + struct Void { + using type = void; + }; + + template <typename T, typename E = void> + struct IsDereferenceable : std::false_type {}; + + template <typename T> + struct IsDereferenceable<T, typename Void<decltype(*std::declval<T>())>::type> + : std::true_type {}; + + template <typename T, + std::enable_if_t<!IsDereferenceable<T>::value>* = nullptr> + static constexpr decltype(auto) Dereference(T& value) { + return value; + } + + // Support dereferenceable types such as std::unique_ptr, std::shared_ptr, raw + // pointers etc. + template <typename T, + std::enable_if_t<IsDereferenceable<T>::value>* = nullptr> + static constexpr decltype(auto) Dereference(T& value) { + return *value; + } + + template <typename T> + static constexpr decltype(auto) Dereference(T* p) { + assert(p != nullptr); + return *p; + } + std::tuple<Manifold0, Manifold1, ManifoldN...> manifolds_; int buffer_size_; std::array<int, kNumManifolds> ambient_sizes_;
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 6210c7a..46cd700 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc
@@ -479,6 +479,24 @@ EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); } +TEST(ProductManifold, Pointers) { + auto p = std::make_unique<QuaternionManifold>(); + auto q = std::make_shared<EuclideanManifold<3>>(); + + ProductManifold<std::unique_ptr<Manifold>, + EuclideanManifold<3>, + std::shared_ptr<EuclideanManifold<3>>> + manifold1{ + std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}, q}; + ProductManifold<QuaternionManifold*, + EuclideanManifold<3>, + std::shared_ptr<EuclideanManifold<3>>> + manifold2{p.get(), EuclideanManifold<3>{}, q}; + + EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize()); + EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize()); +} + TEST(QuaternionManifold, PlusPiBy2) { QuaternionManifold manifold; Vector x = Vector::Zero(4);