Store ProductManifold instances in a tuple
Since the number of manifolds used to initialize ProductManifold and
their types are known at compile-time, it is possible to avoid storing
pointers to the base class as required by a homogeneous, currently
dynamically sized container. Instead, we can use std::tuple<> as a
heterogenous container with the number of elements fixed at compile-time
that allows us to store the concrete manifold realizations.
The advantage of this approach is that we can bypass the vtable when
iterating over each manifold within ProductManifold. The indirection is
invoked only once while accessing the ProductManifoldImpl members.
Additionally, potential dynamic memory allocations by a std::vector can
be completely avoided. This makes the ProductManifold implementation
more efficient both in memory and runtime.
Change-Id: Ic71b0c175ab726f8992e9703f7666bca477baf19
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 4f754c9..5bc5382 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1537,7 +1537,15 @@
.. code-block:: c++
- ProductManifold se3(new QuaternionManifold, new EuclideanManifold<3>);
+ ProductManifold<QuaternionManifold, EuclideanManifold<3>> 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 b3590e9..d42a0d7 100644
--- a/examples/bundle_adjuster.cc
+++ b/examples/bundle_adjuster.cc
@@ -306,7 +306,8 @@
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>>{
+ 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 b67b5e6..c32477d 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 d50de7f..24eac23 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 <algorithm>
#include <array>
#include <memory>
+#include <utility>
#include <vector>
#include "ceres/internal/disable_warnings.h"
@@ -337,72 +338,6 @@
std::vector<bool> 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 <typename... Manifolds>
- 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<Manifold>;
-
- // Wrap all raw pointers into std::unique_ptr for exception safety.
- std::array<ManifoldPtr, kNumManifolds> 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<std::unique_ptr<Manifold>> 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 0000000..bd6131f
--- /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 <algorithm>
+#include <array>
+#include <cstddef>
+#include <numeric>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#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<QuaternionManifold, EuclideanManifold<3>>
+// 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 <typename... Manifolds>
+class ProductManifold final : public Manifold {
+ struct InitTag {};
+
+ public:
+ template <
+ typename Manifold0,
+ typename Manifold1,
+ typename... ManifoldN,
+ std::enable_if_t<std::is_constructible<std::tuple<Manifolds...>,
+ 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>(manifold0),
+ std::forward<Manifold1>(manifold1),
+ std::forward<ManifoldN>(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<kNumManifolds>{});
+ }
+
+ 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<kNumManifolds>{});
+ }
+
+ bool PlusJacobian(const double* x, double* jacobian_ptr) const override {
+ MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize());
+ jacobian.setZero();
+ internal::FixedArray<double> buffer(buffer_size_);
+
+ return PlusJacobianImpl(
+ x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{});
+ }
+
+ bool MinusJacobian(const double* x, double* jacobian_ptr) const override {
+ MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize());
+ jacobian.setZero();
+ internal::FixedArray<double> buffer(buffer_size_);
+
+ return MinusJacobianImpl(
+ x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{});
+ }
+
+ 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 <typename... Args>
+ 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<Args>(manifolds)...} {}
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool PlusImpl(const double* x,
+ const double* delta,
+ double* x_plus_delta,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!std::get<Index0>(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<Indices...>{});
+ }
+
+ static constexpr bool PlusImpl(const double* /*x*/,
+ const double* /*delta*/,
+ double* /*x_plus_delta*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool MinusImpl(const double* y,
+ const double* x,
+ double* y_minus_x,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!std::get<Index0>(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<Indices...>{});
+ }
+
+ static constexpr bool MinusImpl(const double* /*y*/,
+ const double* /*x*/,
+ double* /*y_minus_x*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool PlusJacobianImpl(const double* x,
+ MatrixRef& jacobian,
+ internal::FixedArray<double>& buffer,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!std::get<Index0>(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<Indices...>{});
+ }
+
+ static constexpr bool PlusJacobianImpl(
+ const double* /*x*/,
+ MatrixRef& /*jacobian*/,
+ internal::FixedArray<double>& /*buffer*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool MinusJacobianImpl(const double* x,
+ MatrixRef& jacobian,
+ internal::FixedArray<double>& buffer,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!std::get<Index0>(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<Indices...>{});
+ }
+
+ static constexpr bool MinusJacobianImpl(
+ const double* /*x*/,
+ MatrixRef& /*jacobian*/,
+ internal::FixedArray<double>& /*buffer*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <typename T, std::size_t N>
+ static std::array<T, N> ExclusiveScan(const std::array<T, N>& values) {
+ std::array<T, N> 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<int, kNumManifolds> ambient_sizes_;
+ std::array<int, kNumManifolds> tangent_sizes_;
+ std::array<int, kNumManifolds> ambient_offsets_;
+ std::array<int, kNumManifolds> tangent_offsets_;
+ int ambient_size_;
+ int tangent_size_;
+ std::tuple<Manifolds...> 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 <typename Manifold0, typename Manifold1, typename... Manifolds>
+ProductManifold(Manifold0&&, Manifold1&&, Manifolds&&...)
+ -> ProductManifold<Manifold0, Manifold1, Manifolds...>;
+#endif
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_PRODUCT_MANIFOLD_H_
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc
index b2ff953..f412a79 100644
--- a/internal/ceres/manifold.cc
+++ b/internal/ceres/manifold.cc
@@ -267,112 +267,6 @@
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>(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<double> 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<double> 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 b697d02..60f98d2 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(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<SubsetManifold, SubsetManifold> 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<SubsetManifold, SubsetManifold, SubsetManifold> 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<SubsetManifold,
+ SubsetManifold,
+ SubsetManifold,
+ SubsetManifold>
+ 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<SubsetManifold,
+ SubsetManifold,
+ SubsetManifold,
+ SubsetManifold>
+ manifold(manifold1, manifold2, manifold3, manifold4);
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = Vector::Random(manifold.AmbientSize());
@@ -218,29 +229,29 @@
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, 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<SubsetManifold, EuclideanManifold<2>> manifold(
+ subset_manifold, euclidean_manifold);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 2);
@@ -276,9 +288,10 @@
}
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<EuclideanManifold<2>, SubsetManifold> manifold(
+ euclidean_manifold, subset_manifold);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 2);
@@ -297,6 +310,124 @@
}
}
+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<CopyableManifold, EuclideanManifold<3>> manifold1{
+ CopyableManifold{}, EuclideanManifold<3>{}};
+
+ CopyableManifold inner2;
+ ProductManifold<CopyableManifold, EuclideanManifold<3>> manifold2{
+ inner2, EuclideanManifold<3>{}};
+
+ EXPECT_EQ(manifold1.AmbientSize(), manifold2.AmbientSize());
+ EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize());
+}
+
+TEST(ProductManifold, MoveableOnly) {
+ ProductManifold<MoveableManifold, EuclideanManifold<3>> manifold1{
+ MoveableManifold{}, EuclideanManifold<3>{}};
+
+ MoveableManifold inner2;
+ ProductManifold<MoveableManifold, EuclideanManifold<3>> 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<MoveableManifold, CopyableManifold> manifold1{
+ MoveableManifold{}, inner12};
+
+ MoveableManifold inner21;
+ CopyableManifold inner22;
+ ProductManifold<MoveableManifold, CopyableManifold> 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);
