diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h index 8827b3075..b67b5e66e 100644 --- a/include/ceres/ceres.h +++ b/include/ceres/ceres.h @@ -55,6 +55,7 @@ #include "ceres/gradient_problem_solver.h" #include "ceres/iteration_callback.h" #include "ceres/jet.h" +#include "ceres/line_manifold.h" #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" #include "ceres/manifold.h" @@ -65,6 +66,7 @@ #include "ceres/problem.h" #include "ceres/sized_cost_function.h" #include "ceres/solver.h" +#include "ceres/sphere_manifold.h" #include "ceres/types.h" #include "ceres/version.h" diff --git a/include/ceres/internal/line_manifold.h b/include/ceres/line_manifold.h similarity index 70% rename from include/ceres/internal/line_manifold.h rename to include/ceres/line_manifold.h index aa275e29c..f8f1b2352 100644 --- a/include/ceres/internal/line_manifold.h +++ b/include/ceres/line_manifold.h @@ -29,13 +29,94 @@ // Author: jodebo_beck@gmx.de (Johannes Beck) // -#ifndef CERES_PUBLIC_INTERNAL_LINE_MANIFOLD_H_ -#define CERES_PUBLIC_INTERNAL_LINE_MANIFOLD_H_ +#ifndef CERES_PUBLIC_LINE_MANIFOLD_H_ +#define CERES_PUBLIC_LINE_MANIFOLD_H_ +#include +#include +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/internal/householder_vector.h" #include "ceres/internal/sphere_manifold_functions.h" +#include "ceres/manifold.h" +#include "ceres/types.h" +#include "glog/logging.h" namespace ceres { +// This provides a manifold for lines, where the line is +// over-parameterized by an origin point and a direction vector. So the +// parameter vector size needs to be two times the ambient space dimension, +// where the first half is interpreted as the origin point and the second half +// as the direction. +// +// The plus operator for the line direction is the same as for the +// SphereManifold. The update of the origin point is +// perpendicular to the line direction before the update. +// +// This manifold is a special case of the affine Grassmannian +// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) +// for the case Graff_1(R^n). +// +// The class works with dynamic and static ambient space dimensions. If the +// 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: +// +// LineManifold manifold(ambient_dim); +// +template +class LineManifold final : public Manifold { + public: + static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2, + "The ambient space must be at least 2."); + static_assert(ceres::DYNAMIC == Eigen::Dynamic, + "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); + + LineManifold(); + explicit LineManifold(int size); + + int AmbientSize() const override { return 2 * size_; } + int TangentSize() const override { return 2 * (size_ - 1); } + 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: + static constexpr bool IsDynamic = (AmbientSpaceDimension == ceres::DYNAMIC); + static constexpr int TangentSpaceDimension = + IsDynamic ? ceres::DYNAMIC : AmbientSpaceDimension - 1; + + static constexpr int DAmbientSpaceDimension = + IsDynamic ? ceres::DYNAMIC : 2 * AmbientSpaceDimension; + static constexpr int DTangentSpaceDimension = + IsDynamic ? ceres::DYNAMIC : 2 * TangentSpaceDimension; + + using AmbientVector = Eigen::Matrix; + using TangentVector = Eigen::Matrix; + using MatrixPlusJacobian = Eigen::Matrix; + using MatrixMinusJacobian = Eigen::Matrix; + + const int size_{AmbientSpaceDimension}; +}; template LineManifold::LineManifold() @@ -216,4 +297,8 @@ bool LineManifold::MinusJacobian( } // namespace ceres -#endif // CERES_PUBLIC_INTERNAL_LINE_MANIFOLD_H_ \ No newline at end of file +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#endif // CERES_PUBLIC_LINE_MANIFOLD_H_ diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h index 5c7af2443..d50de7ff1 100644 --- a/include/ceres/manifold.h +++ b/include/ceres/manifold.h @@ -467,163 +467,10 @@ class CERES_EXPORT EigenQuaternionManifold final : public Manifold { bool MinusJacobian(const double* x, double* jacobian) const override; }; -// This provides a manifold on a sphere meaning that the norm of the vector -// stays the same. Such cases often arises in Structure for Motion -// problems. One example where they are used is in representing points whose -// triangulation is ill-conditioned. Here it is advantageous to use an -// over-parameterization since homogeneous vectors can represent points at -// infinity. -// -// The plus operator is defined as -// Plus(x, delta) = -// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x -// -// The minus operator is defined as -// Minus(x, y) = 2 atan2(nhy, y[-1]) / nhy * hy[0 : size_ - 1] -// with nhy = norm(hy[0 : size_ - 1]) -// -// with * defined as an operator which applies the update orthogonal to x to -// remain on the sphere. 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 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: -// -// 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. -// 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 == ceres::DYNAMIC || AmbientSpaceDimension > 1, - "The size of the homogeneous vector needs to be greater than 1."); - static_assert(ceres::DYNAMIC == Eigen::Dynamic, - "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); - - SphereManifold(); - explicit SphereManifold(int size); - - int AmbientSize() const override { - return AmbientSpaceDimension == ceres::DYNAMIC ? size_ - : AmbientSpaceDimension; - } - int TangentSize() const override { return AmbientSize() - 1; } - - 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: - static constexpr int TangentSpaceDimension = - AmbientSpaceDimension > 0 ? AmbientSpaceDimension - 1 : Eigen::Dynamic; - - using AmbientVector = Eigen::Matrix; - using TangentVector = Eigen::Matrix; - using MatrixPlusJacobian = Eigen::Matrix; - using MatrixMinusJacobian = Eigen::Matrix; - - const int size_{}; -}; - -// This provides a manifold for lines, where the line is -// over-parameterized by an origin point and a direction vector. So the -// parameter vector size needs to be two times the ambient space dimension, -// where the first half is interpreted as the origin point and the second half -// as the direction. -// -// The plus operator for the line direction is the same as for the -// SphereManifold. The update of the origin point is -// perpendicular to the line direction before the update. -// -// This manifold is a special case of the affine Grassmannian -// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) -// for the case Graff_1(R^n). -// -// The class works with dynamic and static ambient space dimensions. If the -// 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: -// -// LineManifold manifold(ambient_dim); -// -template -class LineManifold final : public Manifold { - public: - static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2, - "The ambient space must be at least 2."); - static_assert(ceres::DYNAMIC == Eigen::Dynamic, - "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); - - LineManifold(); - explicit LineManifold(int size); - - int AmbientSize() const override { return 2 * size_; } - int TangentSize() const override { return 2 * (size_ - 1); } - 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: - static constexpr bool IsDynamic = (AmbientSpaceDimension == ceres::DYNAMIC); - static constexpr int TangentSpaceDimension = - IsDynamic ? ceres::DYNAMIC : AmbientSpaceDimension - 1; - - static constexpr int DAmbientSpaceDimension = - IsDynamic ? ceres::DYNAMIC : 2 * AmbientSpaceDimension; - static constexpr int DTangentSpaceDimension = - IsDynamic ? ceres::DYNAMIC : 2 * TangentSpaceDimension; - - using AmbientVector = Eigen::Matrix; - using TangentVector = Eigen::Matrix; - using MatrixPlusJacobian = Eigen::Matrix; - using MatrixMinusJacobian = Eigen::Matrix; - - const int size_{AmbientSpaceDimension}; -}; - } // namespace ceres -#include "internal/line_manifold.h" -#include "internal/sphere_manifold.h" - // clang-format off #include "ceres/internal/reenable_warnings.h" +// clang-format on #endif // CERES_PUBLIC_MANIFOLD_H_ diff --git a/include/ceres/internal/sphere_manifold.h b/include/ceres/sphere_manifold.h similarity index 57% rename from include/ceres/internal/sphere_manifold.h rename to include/ceres/sphere_manifold.h index 140022841..c2ad5d333 100644 --- a/include/ceres/internal/sphere_manifold.h +++ b/include/ceres/sphere_manifold.h @@ -29,14 +29,105 @@ // Author: vitus@google.com (Mike Vitus) // jodebo_beck@gmx.de (Johannes Beck) -#ifndef CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_H_ -#define CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_H_ +#ifndef CERES_PUBLIC_SPHERE_MANIFOLD_H_ +#define CERES_PUBLIC_SPHERE_MANIFOLD_H_ +#include +#include +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/internal/householder_vector.h" #include "ceres/internal/sphere_manifold_functions.h" +#include "ceres/manifold.h" +#include "ceres/types.h" +#include "glog/logging.h" namespace ceres { +// This provides a manifold on a sphere meaning that the norm of the vector +// stays the same. Such cases often arises in Structure for Motion +// problems. One example where they are used is in representing points whose +// triangulation is ill-conditioned. Here it is advantageous to use an +// over-parameterization since homogeneous vectors can represent points at +// infinity. +// +// The plus operator is defined as +// Plus(x, delta) = +// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x +// +// The minus operator is defined as +// Minus(x, y) = 2 atan2(nhy, y[-1]) / nhy * hy[0 : size_ - 1] +// with nhy = norm(hy[0 : size_ - 1]) +// +// with * defined as an operator which applies the update orthogonal to x to +// remain on the sphere. 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 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: +// +// 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. +// 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 == ceres::DYNAMIC || AmbientSpaceDimension > 1, + "The size of the homogeneous vector needs to be greater than 1."); + static_assert(ceres::DYNAMIC == Eigen::Dynamic, + "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); + + SphereManifold(); + explicit SphereManifold(int size); + + int AmbientSize() const override { + return AmbientSpaceDimension == ceres::DYNAMIC ? size_ + : AmbientSpaceDimension; + } + int TangentSize() const override { return AmbientSize() - 1; } + + 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: + static constexpr int TangentSpaceDimension = + AmbientSpaceDimension > 0 ? AmbientSpaceDimension - 1 : Eigen::Dynamic; + + using AmbientVector = Eigen::Matrix; + using TangentVector = Eigen::Matrix; + using MatrixPlusJacobian = Eigen::Matrix; + using MatrixMinusJacobian = Eigen::Matrix; + + const int size_{}; +}; + template SphereManifold::SphereManifold() : size_{AmbientSpaceDimension} { @@ -130,6 +221,11 @@ bool SphereManifold::MinusJacobian( internal::ComputeSphereManifoldMinusJacobian(x, &jacobian); return true; } + } // namespace ceres -#endif +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#endif // CERES_PUBLIC_SPHERE_MANIFOLD_H_ diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 99965afc7..b697d0278 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc @@ -38,9 +38,11 @@ #include "ceres/dynamic_numeric_diff_cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" +#include "ceres/line_manifold.h" #include "ceres/manifold_test_utils.h" #include "ceres/numeric_diff_options.h" #include "ceres/rotation.h" +#include "ceres/sphere_manifold.h" #include "ceres/types.h" #include "gmock/gmock.h" #include "gtest/gtest.h"