diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 950377401..be8e9587b 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst @@ -1475,6 +1475,21 @@ no need to use this manifold on its own. It is provided for the 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 manifold(ambient_dim); + :class:`SubsetManifold` ----------------------- @@ -1521,7 +1536,7 @@ manifold can be constructed as: .. code-block:: c++ - ProductManifold se3(new QuaternionManifold(), new EuclideanManifold(3)); + ProductManifold se3(new QuaternionManifold, new EuclideanManifold<3>); :class:`QuaternionManifold` @@ -1617,7 +1632,7 @@ points at infinity. 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 9727137ab..b3590e936 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc @@ -306,7 +306,7 @@ void BuildProblem(BALProblem* bal_problem, Problem* problem) { 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 458eec059..5c7af2443 100644 --- a/include/ceres/manifold.h +++ b/include/ceres/manifold.h @@ -32,6 +32,7 @@ #define CERES_PUBLIC_MANIFOLD_H_ #include +#include #include #include #include @@ -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 @@ class CERES_EXPORT Manifold { // 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 manifold(ambient_dim); +template 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 x(x_ptr, size_); + Eigen::Map delta(delta_ptr, size_); + Eigen::Map 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 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 x(x_ptr, size_); + Eigen::Map y(y_ptr, size_); + Eigen::Map 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 jacobian(jacobian_ptr, size_, size_); + jacobian.setIdentity(); + return true; + } private: - int size_ = 0; + static constexpr bool IsDynamic = (Size == ceres::DYNAMIC); + using AmbientVector = Eigen::Matrix; + using MatrixJacobian = Eigen::Matrix; + + int size_{}; }; // Hold a subset of the parameters inside a parameter block constant. @@ -271,22 +338,22 @@ class CERES_EXPORT SubsetManifold final : public Manifold { }; // 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 @@ class CERES_EXPORT ProductManifold final : public Manifold { }; // 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 @@ class CERES_EXPORT ProductManifold final : public Manifold { // // 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 @@ class CERES_EXPORT QuaternionManifold final : public Manifold { 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 @@ class CERES_EXPORT EigenQuaternionManifold final : public Manifold { // 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 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 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 @@ class SphereManifold final : public Manifold { // 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 manifold(ambient_dim); // @@ -510,7 +577,7 @@ class LineManifold final : public Manifold { 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 @@ class LineManifold final : public Manifold { 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; using TangentVector = Eigen::Matrix; diff --git a/internal/ceres/gradient_problem.cc b/internal/ceres/gradient_problem.cc index 93b18f567..cdd472fe8 100644 --- a/internal/ceres/gradient_problem.cc +++ b/internal/ceres/gradient_problem.cc @@ -40,8 +40,8 @@ namespace ceres { GradientProblem::GradientProblem(FirstOrderFunction* function) : function_(function), - manifold_( - std::make_unique(function_->NumParameters())), + manifold_(std::make_unique>( + function_->NumParameters())), scratch_(new double[function_->NumParameters()]) { CHECK(function != nullptr); } @@ -56,7 +56,8 @@ GradientProblem::GradientProblem(FirstOrderFunction* function, manifold_ = std::make_unique(parameterization_.get()); } else { - manifold_ = std::make_unique(function_->NumParameters()); + manifold_ = std::make_unique>( + function_->NumParameters()); } CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize()); } @@ -65,9 +66,12 @@ GradientProblem::GradientProblem(FirstOrderFunction* function, 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>( + 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 13508baea..47f4348fa 100644 --- a/internal/ceres/gradient_problem_test.cc +++ b/internal/ceres/gradient_problem_test.cc @@ -107,7 +107,7 @@ TEST(GradientProblem, EvaluationWithParameterizationAndWithGradient) { 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, EvaluationWithoutManifoldAndWithGradient) { 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 1f4781eae..b2ff9538c 100644 --- a/internal/ceres/manifold.cc +++ b/internal/ceres/manifold.cc @@ -159,51 +159,6 @@ bool Manifold::RightMultiplyByPlusJacobian(const double* x, 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& constant_parameters) diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 52821b959..99965afc7 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc @@ -51,8 +51,29 @@ namespace internal { 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 manifold(3); EXPECT_EQ(manifold.AmbientSize(), 3); EXPECT_EQ(manifold.TangentSize(), 3); @@ -230,7 +251,7 @@ TEST(ProductManifold, NormalFunctionTest) { 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, ZeroTangentSizeAndEuclidean) { 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 641dad917..f424c7e36 100644 --- a/internal/ceres/problem_test.cc +++ b/internal/ceres/problem_test.cc @@ -67,7 +67,6 @@ class UnaryCostFunction : public CostFunction { 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 @@ TEST(Problem, SetManifoldWithUnknownPtrDies) { 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 @@ TEST(Problem, GetManifold) { 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 @@ TEST(Problem, HasManifold) { 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));