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);
