From 6a37fbf9b4bf4ac949d4fa35c2a5710b36f30ff6 Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Mon, 28 Feb 2022 11:23:43 -0800 Subject: [PATCH] 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 --- docs/source/nnls_modeling.rst | 19 ++- examples/bundle_adjuster.cc | 2 +- include/ceres/manifold.h | 157 +++++++++++++++++------- internal/ceres/gradient_problem.cc | 16 ++- internal/ceres/gradient_problem_test.cc | 4 +- internal/ceres/manifold.cc | 45 ------- internal/ceres/manifold_test.cc | 29 ++++- internal/ceres/problem_test.cc | 7 +- 8 files changed, 170 insertions(+), 109 deletions(-) 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));