Add static/compile time sizing to EuclideanManifold
This brings it in line with other manifolds like SphereManifold
and LineManifold, where the user has the choice to specify the size
of the manifold at compile time or runtime.
Most of the time the size is known at compile time so this will
speed up the common case.
Change-Id: I0c7ff8b7a9a64a81203eb11afc074874e208815a
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 9503774..be8e958 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1475,6 +1475,21 @@
purpose of testing and for use in combination with other manifolds
using :class:`ProductManifold`.
+The class works with dynamic and static ambient space dimensions. If
+the ambient space dimensions is known at compile time use
+
+.. code-block:: c++
+
+ EuclideanManifold<3> manifold;
+
+If the ambient space dimensions is not known at compile time the
+template parameter needs to be set to `ceres::DYNAMIC` and the actual
+dimension needs to be provided as a constructor argument:
+
+.. code-block:: c++
+
+ EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim);
+
:class:`SubsetManifold`
-----------------------
@@ -1521,7 +1536,7 @@
.. code-block:: c++
- ProductManifold se3(new QuaternionManifold(), new EuclideanManifold(3));
+ ProductManifold se3(new QuaternionManifold, new EuclideanManifold<3>);
:class:`QuaternionManifold`
@@ -1617,7 +1632,7 @@
The ambient space dimension is required to be greater than 1.
The class works with dynamic and static ambient space dimensions. If
-the ambient space dimensions is know at compile time use
+the ambient space dimensions is known at compile time use
.. code-block:: c++
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc
index 9727137..b3590e9 100644
--- a/examples/bundle_adjuster.cc
+++ b/examples/bundle_adjuster.cc
@@ -306,7 +306,7 @@
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(new QuaternionManifold, new 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/manifold.h b/include/ceres/manifold.h
index 458eec0..5c7af24 100644
--- a/include/ceres/manifold.h
+++ b/include/ceres/manifold.h
@@ -32,6 +32,7 @@
#define CERES_PUBLIC_MANIFOLD_H_
#include <Eigen/Core>
+#include <algorithm>
#include <array>
#include <memory>
#include <vector>
@@ -39,6 +40,7 @@
#include "ceres/internal/disable_warnings.h"
#include "ceres/internal/export.h"
#include "ceres/types.h"
+#include "glog/logging.h"
namespace ceres {
@@ -223,26 +225,91 @@
// subtraction:
// Plus(x, delta) = x + delta
// Minus(y, x) = y - x.
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is know at compile time use
+//
+// EuclideanManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
+//
+// EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim);
+template <int Size>
class CERES_EXPORT EuclideanManifold final : public Manifold {
public:
- explicit EuclideanManifold(int size);
- 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;
+ static_assert(Size == ceres::DYNAMIC || Size >= 0,
+ "The size of the manifold needs to be non-negative.");
+ static_assert(ceres::DYNAMIC == Eigen::Dynamic,
+ "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+ EuclideanManifold() : size_{Size} {
+ static_assert(
+ Size != ceres::DYNAMIC,
+ "The size is set to dynamic. Please call the constructor with a size.");
+ }
+
+ explicit EuclideanManifold(int size) : size_(size) {
+ if (Size != ceres::DYNAMIC) {
+ CHECK_EQ(Size, size)
+ << "Specified size by template parameter differs from the supplied "
+ "one.";
+ } else {
+ CHECK_GE(size_, 0)
+ << "The size of the manifold needs to be non-negative.";
+ }
+ }
+
+ int AmbientSize() const override { return size_; }
+ int TangentSize() const override { return size_; }
+
+ bool Plus(const double* x_ptr,
+ const double* delta_ptr,
+ double* x_plus_delta_ptr) const override {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<const AmbientVector> delta(delta_ptr, size_);
+ Eigen::Map<AmbientVector> x_plus_delta(x_plus_delta_ptr, size_);
+ x_plus_delta = x + delta;
+ return true;
+ }
+
+ bool PlusJacobian(const double* x_ptr, double* jacobian_ptr) const override {
+ Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_);
+ jacobian.setIdentity();
+ return true;
+ }
+
bool RightMultiplyByPlusJacobian(const double* x,
const int num_rows,
const double* ambient_matrix,
- double* tangent_matrix) 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;
+ double* tangent_matrix) const override {
+ std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix);
+ return true;
+ }
+
+ bool Minus(const double* y_ptr,
+ const double* x_ptr,
+ double* y_minus_x_ptr) const override {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<const AmbientVector> y(y_ptr, size_);
+ Eigen::Map<AmbientVector> y_minus_x(y_minus_x_ptr, size_);
+ y_minus_x = y - x;
+ return true;
+ }
+
+ bool MinusJacobian(const double* x_ptr, double* jacobian_ptr) const override {
+ Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_);
+ jacobian.setIdentity();
+ return true;
+ }
private:
- int size_ = 0;
+ static constexpr bool IsDynamic = (Size == ceres::DYNAMIC);
+ using AmbientVector = Eigen::Matrix<double, Size, 1>;
+ using MatrixJacobian = Eigen::Matrix<double, Size, Size, Eigen::RowMajor>;
+
+ int size_{};
};
// Hold a subset of the parameters inside a parameter block constant.
@@ -271,22 +338,22 @@
};
// 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.
+// 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.
+// 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.
+ // 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
@@ -337,8 +404,8 @@
};
// 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.
+// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as
+// unit norm 4-vectors, i.e.
//
// q = [q0; q1; q2; q3], |q| = 1
//
@@ -351,8 +418,8 @@
//
// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
//
-// The tangent space is R^3, which relates to the ambient space through the Plus
-// and Minus operations defined as:
+// The tangent space is R^3, which relates to the ambient space through the
+// Plus and Minus operations defined as:
//
// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x
// Minus(y, x) = to_delta(y * x^{-1})
@@ -376,12 +443,12 @@
bool MinusJacobian(const double* x, double* jacobian) const override;
};
-// Implements the quaternion manifold for Eigen's representation of the Hamilton
-// quaternion. Geometrically it is exactly the same as the QuaternionManifold
-// defined above. However, Eigen uses a different internal memory layout for the
-// elements of the quaternion than what is commonly used. It stores the
-// quaternion in memory as [q1, q2, q3, q0] or [x, y, z, w] where the real
-// (scalar) part is last.
+// Implements the quaternion manifold for Eigen's representation of the
+// Hamilton quaternion. Geometrically it is exactly the same as the
+// QuaternionManifold defined above. However, Eigen uses a different internal
+// memory layout for the elements of the quaternion than what is commonly
+// used. It stores the quaternion in memory as [q1, q2, q3, q0] or
+// [x, y, z, w] where the real (scalar) part is last.
//
// Since Ceres operates on parameter blocks which are raw double pointers this
// difference is important and requires a different manifold.
@@ -420,27 +487,27 @@
// than 1.
//
// The class works with dynamic and static ambient space dimensions. If the
-// ambient space dimensions is know at compile time use
+// ambient space dimensions is known at compile time use
//
// SphereManifold<3> manifold;
//
// If the ambient space dimensions is not known at compile time the template
-// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs to
-// be provided as a constructor argument:
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
//
// SphereManifold<ceres::DYNAMIC> manifold(ambient_dim);
//
-// See section B.2 (p.25) in "Integrating Generic Sensor Fusion Algorithms with
-// Sound State Representations through Encapsulation of Manifolds" by C.
+// See section B.2 (p.25) in "Integrating Generic Sensor Fusion Algorithms
+// with Sound State Representations through Encapsulation of Manifolds" by C.
// Hertzberg, R. Wagner, U. Frese and L. Schroder for more details
// (https://arxiv.org/pdf/1107.1119.pdf)
template <int AmbientSpaceDimension>
class SphereManifold final : public Manifold {
public:
static_assert(
- AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension > 1,
+ AmbientSpaceDimension == ceres::DYNAMIC || AmbientSpaceDimension > 1,
"The size of the homogeneous vector needs to be greater than 1.");
- static_assert(DYNAMIC == Eigen::Dynamic,
+ static_assert(ceres::DYNAMIC == Eigen::Dynamic,
"ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
SphereManifold();
@@ -495,13 +562,13 @@
// for the case Graff_1(R^n).
//
// The class works with dynamic and static ambient space dimensions. If the
-// ambient space dimensions is know at compile time use
+// ambient space dimensions is known at compile time use
//
// LineManifold<3> manifold;
//
// If the ambient space dimensions is not known at compile time the template
-// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs to
-// be provided as a constructor argument:
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
//
// LineManifold<ceres::DYNAMIC> manifold(ambient_dim);
//
@@ -510,7 +577,7 @@
public:
static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2,
"The ambient space must be at least 2.");
- static_assert(DYNAMIC == Eigen::Dynamic,
+ static_assert(ceres::DYNAMIC == Eigen::Dynamic,
"ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
LineManifold();
@@ -528,14 +595,14 @@
bool MinusJacobian(const double* x, double* jacobian) const override;
private:
- static constexpr int IsDynamic = (AmbientSpaceDimension == Eigen::Dynamic);
+ static constexpr bool IsDynamic = (AmbientSpaceDimension == ceres::DYNAMIC);
static constexpr int TangentSpaceDimension =
- IsDynamic ? Eigen::Dynamic : AmbientSpaceDimension - 1;
+ IsDynamic ? ceres::DYNAMIC : AmbientSpaceDimension - 1;
static constexpr int DAmbientSpaceDimension =
- IsDynamic ? Eigen::Dynamic : 2 * AmbientSpaceDimension;
+ IsDynamic ? ceres::DYNAMIC : 2 * AmbientSpaceDimension;
static constexpr int DTangentSpaceDimension =
- IsDynamic ? Eigen::Dynamic : 2 * TangentSpaceDimension;
+ IsDynamic ? ceres::DYNAMIC : 2 * TangentSpaceDimension;
using AmbientVector = Eigen::Matrix<double, AmbientSpaceDimension, 1>;
using TangentVector = Eigen::Matrix<double, TangentSpaceDimension, 1>;
diff --git a/internal/ceres/gradient_problem.cc b/internal/ceres/gradient_problem.cc
index 93b18f5..cdd472f 100644
--- a/internal/ceres/gradient_problem.cc
+++ b/internal/ceres/gradient_problem.cc
@@ -40,8 +40,8 @@
GradientProblem::GradientProblem(FirstOrderFunction* function)
: function_(function),
- manifold_(
- std::make_unique<EuclideanManifold>(function_->NumParameters())),
+ manifold_(std::make_unique<EuclideanManifold<DYNAMIC>>(
+ function_->NumParameters())),
scratch_(new double[function_->NumParameters()]) {
CHECK(function != nullptr);
}
@@ -56,7 +56,8 @@
manifold_ =
std::make_unique<internal::ManifoldAdapter>(parameterization_.get());
} else {
- manifold_ = std::make_unique<EuclideanManifold>(function_->NumParameters());
+ manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>(
+ function_->NumParameters());
}
CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize());
}
@@ -65,9 +66,12 @@
Manifold* manifold)
: function_(function), scratch_(new double[function_->NumParameters()]) {
CHECK(function != nullptr);
- manifold_.reset(manifold != nullptr
- ? manifold
- : new EuclideanManifold(function_->NumParameters()));
+ if (manifold != nullptr) {
+ manifold_.reset(manifold);
+ } else {
+ manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>(
+ function_->NumParameters());
+ }
CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize());
}
diff --git a/internal/ceres/gradient_problem_test.cc b/internal/ceres/gradient_problem_test.cc
index 13508ba..47f4348 100644
--- a/internal/ceres/gradient_problem_test.cc
+++ b/internal/ceres/gradient_problem_test.cc
@@ -107,7 +107,7 @@
TEST(GradientProblem, EvalutaionWithManifoldAndNoGradient) {
ceres::GradientProblem problem(new QuadraticTestFunction(),
- new EuclideanManifold(1));
+ new EuclideanManifold<1>);
double x = 7.0;
double cost = 0;
problem.Evaluate(&x, &cost, nullptr);
@@ -125,7 +125,7 @@
TEST(GradientProblem, EvaluationWithManifoldAndWithGradient) {
ceres::GradientProblem problem(new QuadraticTestFunction(),
- new EuclideanManifold(1));
+ new EuclideanManifold<1>);
double x = 7.0;
double cost = 0;
double gradient = 0;
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc
index 1f4781e..b2ff953 100644
--- a/internal/ceres/manifold.cc
+++ b/internal/ceres/manifold.cc
@@ -159,51 +159,6 @@
return true;
}
-EuclideanManifold::EuclideanManifold(int size) : size_(size) {
- CHECK_GE(size, 0);
-}
-
-int EuclideanManifold::AmbientSize() const { return size_; }
-
-int EuclideanManifold::TangentSize() const { return size_; }
-
-bool EuclideanManifold::Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const {
- for (int i = 0; i < size_; ++i) {
- x_plus_delta[i] = x[i] + delta[i];
- }
- return true;
-}
-
-bool EuclideanManifold::PlusJacobian(const double* x, double* jacobian) const {
- MatrixRef(jacobian, size_, size_).setIdentity();
- return true;
-}
-
-bool EuclideanManifold::RightMultiplyByPlusJacobian(
- const double* x,
- const int num_rows,
- const double* ambient_matrix,
- double* tangent_matrix) const {
- std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix);
- return true;
-}
-
-bool EuclideanManifold::Minus(const double* y,
- const double* x,
- double* y_minus_x) const {
- for (int i = 0; i < size_; ++i) {
- y_minus_x[i] = y[i] - x[i];
- }
- return true;
-};
-
-bool EuclideanManifold::MinusJacobian(const double* x, double* jacobian) const {
- MatrixRef(jacobian, size_, size_).setIdentity();
- return true;
-}
-
SubsetManifold::SubsetManifold(const int size,
const std::vector<int>& constant_parameters)
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
index 52821b9..99965af 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -51,8 +51,29 @@
constexpr int kNumTrials = 1000;
constexpr double kTolerance = 1e-9;
-TEST(EuclideanManifold, NormalFunctionTest) {
- EuclideanManifold manifold(3);
+TEST(EuclideanManifold, StaticNormalFunctionTest) {
+ EuclideanManifold<3> manifold;
+ EXPECT_EQ(manifold.AmbientSize(), 3);
+ EXPECT_EQ(manifold.TangentSize(), 3);
+
+ Vector zero_tangent = Vector::Zero(manifold.TangentSize());
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = Vector::Random(manifold.AmbientSize());
+ const Vector y = Vector::Random(manifold.AmbientSize());
+ Vector delta = Vector::Random(manifold.TangentSize());
+ Vector x_plus_delta = Vector::Zero(manifold.AmbientSize());
+
+ manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
+ EXPECT_NEAR((x_plus_delta - x - delta).norm() / (x + delta).norm(),
+ 0.0,
+ kTolerance);
+
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+TEST(EuclideanManifold, DynamicNormalFunctionTest) {
+ EuclideanManifold<DYNAMIC> manifold(3);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 3);
@@ -230,7 +251,7 @@
TEST(ProductManifold, ZeroTangentSizeAndEuclidean) {
Manifold* subset_manifold = new SubsetManifold(1, {0});
- Manifold* euclidean_manifold = new EuclideanManifold(2);
+ Manifold* euclidean_manifold = new EuclideanManifold<2>;
ProductManifold manifold(subset_manifold, euclidean_manifold);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 2);
@@ -254,7 +275,7 @@
TEST(ProductManifold, EuclideanAndZeroTangentSize) {
Manifold* subset_manifold = new SubsetManifold(1, {0});
- Manifold* euclidean_manifold = new EuclideanManifold(2);
+ Manifold* euclidean_manifold = new EuclideanManifold<2>;
ProductManifold manifold(euclidean_manifold, subset_manifold);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 2);
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 641dad9..f424c7e 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -67,7 +67,6 @@
mutable_parameter_block_sizes()->push_back(parameter_block_size);
}
-
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const final {
@@ -595,7 +594,7 @@
Problem problem;
problem.AddParameterBlock(x, 3);
- EXPECT_DEATH_IF_SUPPORTED(problem.SetManifold(y, new EuclideanManifold(3)),
+ EXPECT_DEATH_IF_SUPPORTED(problem.SetManifold(y, new EuclideanManifold<3>),
"Parameter block not found:");
}
@@ -632,7 +631,7 @@
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
- Manifold* manifold = new EuclideanManifold(3);
+ Manifold* manifold = new EuclideanManifold<3>;
problem.SetManifold(x, manifold);
EXPECT_EQ(problem.GetManifold(x), manifold);
EXPECT_TRUE(problem.GetManifold(y) == nullptr);
@@ -660,7 +659,7 @@
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
- Manifold* manifold = new EuclideanManifold(3);
+ Manifold* manifold = new EuclideanManifold<3>;
problem.SetManifold(x, manifold);
EXPECT_TRUE(problem.HasManifold(x));
EXPECT_FALSE(problem.HasManifold(y));
