From b0aef211db734379319c19c030e734d6e23436b0 Mon Sep 17 00:00:00 2001
From: Sergiu Deitsch <sergiu.deitsch@gmail.com>
Date: Thu, 3 Mar 2022 16:08:34 +0100
Subject: [PATCH] Allow to store pointers in ProductManifold

Change-Id: I32df7afab3a195efb0407b0d8f35dcd2d7cb95d2
---
 docs/source/nnls_modeling.rst    |  7 ++++
 include/ceres/product_manifold.h | 60 +++++++++++++++++++++++++++-----
 internal/ceres/manifold_test.cc  | 18 ++++++++++
 3 files changed, 76 insertions(+), 9 deletions(-)

diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 51afcece1..c6b7987ce 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1548,6 +1548,13 @@ Manifolds can be copied and moved to :class:`ProductManifold`:
    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 b7ebe4d42..33f046da2 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 @@ namespace ceres {
 // 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 @@ class ProductManifold final : public Manifold {
   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 @@ class ProductManifold final : public Manifold {
                 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 @@ class ProductManifold final : public Manifold {
                  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 @@ class ProductManifold final : public Manifold {
                         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 @@ class ProductManifold final : public Manifold {
                          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 @@ class ProductManifold final : public Manifold {
     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 6210c7a74..46cd700c8 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -479,6 +479,24 @@ TEST(ProductManifold, DefaultConstructible) {
   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);