From 9792bec7a00f8f8c794080c8e3650f79490cb558 Mon Sep 17 00:00:00 2001 From: hauke strasdat Date: Thu, 9 Nov 2023 12:33:16 -0800 Subject: [PATCH] docs: api-reference for sophus::manifold --- cpp/sophus/common/common.h | 4 +- cpp/sophus/linalg/vector_space_traits.h | 63 +++++----- cpp/sophus/manifold/CMakeLists.txt | 1 + cpp/sophus/manifold/complex.h | 65 +++++++++++ cpp/sophus/manifold/product_manifold.h | 50 +++++--- cpp/sophus/manifold/quaternion.h | 97 ++++++++++++++-- cpp/sophus/manifold/unit_vector.h | 108 +++++++++++++++--- cpp/sophus/manifold/vector_manifold.h | 20 +++- cpp/sophus/manifold/vector_manifold_test.cpp | 19 +++ .../doxyfile_cpp_farm_ng_warn_as_error | 3 +- 10 files changed, 346 insertions(+), 84 deletions(-) create mode 100644 cpp/sophus/manifold/vector_manifold_test.cpp diff --git a/cpp/sophus/common/common.h b/cpp/sophus/common/common.h index 0beb62fe..70389f6d 100644 --- a/cpp/sophus/common/common.h +++ b/cpp/sophus/common/common.h @@ -25,7 +25,7 @@ #include #include -// \cond HIDDEN_SYMBOLS +// BEGIN(exclude from doxygen) \cond HIDDEN_SYMBOLS // from cd #define SOPHUS_FORMAT(...) FARM_FORMAT(__VA_ARGS__) @@ -47,7 +47,7 @@ // from #define SOPHUS_TRY(...) FARM_TRY(__VA_ARGS__) #define SOPHUS_UNEXPECTED(...) FARM_UNEXPECTED(__VA_ARGS__) -// \endcond +// END(exclude from doxygen) \endcond namespace sophus { diff --git a/cpp/sophus/linalg/vector_space_traits.h b/cpp/sophus/linalg/vector_space_traits.h index 82605e1f..e7371cd4 100644 --- a/cpp/sophus/linalg/vector_space_traits.h +++ b/cpp/sophus/linalg/vector_space_traits.h @@ -22,43 +22,6 @@ namespace sophus { template struct PointTraits; -/// Trait for a scalar valued point. -/// -/// Mathematically, we consider a point an element of a "vector space". In this -/// case, the vector space is one-dimensional. -template -struct PointTraits { - /// Scalar type - using Scalar = TPoint; - - /// Is floating point number? - static bool constexpr kIsFloatingPoint = std::is_floating_point_v; - /// Is integer number? - static bool constexpr kIsInteger = std::is_integral_v; - - /// number of rows - this is a scalar hence always 1 - static int constexpr kRows = 1; - /// number of columns - this is a scalar hence always 1 - static int constexpr kCols = 1; - - /// Point set contains infinity? - static bool constexpr kHasInfinity = - std::numeric_limits::has_infinity; - /// Point set contains quite NAN? - static bool constexpr kHasQuietNan = - std::numeric_limits::has_quiet_NaN; - /// Point set contains signaling NAN? - static bool constexpr kHasSignalingNan = - std::numeric_limits::has_signaling_NaN; - - /// Lowest value (e.g. typically large negative number) - static TPoint lowest() { return std::numeric_limits::lowest(); }; - /// Smallest positive value - static TPoint min() { return std::numeric_limits::min(); }; - /// Greatest value - static TPoint max() { return std::numeric_limits::max(); }; -}; - /// Trait for a multi-dimensional point. /// /// Mathematically, we consider a point an element of a "vector space". @@ -106,4 +69,30 @@ struct PointTraits { /// ... plus a bunch more if we need them }; +// BEGIN(exclude from doxygen) \cond HIDDEN_SYMBOLS + +// Single scalar specialization (1-dim case) of PointTraits +template +struct PointTraits { + using Scalar = TPoint; + + static bool constexpr kIsFloatingPoint = std::is_floating_point_v; + static bool constexpr kIsInteger = std::is_integral_v; + + static int constexpr kRows = 1; + static int constexpr kCols = 1; + + static bool constexpr kHasInfinity = + std::numeric_limits::has_infinity; + static bool constexpr kHasQuietNan = + std::numeric_limits::has_quiet_NaN; + static bool constexpr kHasSignalingNan = + std::numeric_limits::has_signaling_NaN; + + static TPoint lowest() { return std::numeric_limits::lowest(); }; + static TPoint min() { return std::numeric_limits::min(); }; + static TPoint max() { return std::numeric_limits::max(); }; +}; +// END(exclude from doxygen) \endcond + } // namespace sophus diff --git a/cpp/sophus/manifold/CMakeLists.txt b/cpp/sophus/manifold/CMakeLists.txt index 8fe5e820..4c900da3 100644 --- a/cpp/sophus/manifold/CMakeLists.txt +++ b/cpp/sophus/manifold/CMakeLists.txt @@ -10,6 +10,7 @@ set(sophus_manifold_src_prefixes product_manifold quaternion unit_vector + vector_manifold ) set(sophus_manifold_h) diff --git a/cpp/sophus/manifold/complex.h b/cpp/sophus/manifold/complex.h index 31a9b191..f6c8f9a3 100644 --- a/cpp/sophus/manifold/complex.h +++ b/cpp/sophus/manifold/complex.h @@ -13,15 +13,26 @@ namespace sophus { +/// Generic complex number implementation +/// +/// Impl class without any storage, but only static methods. template class ComplexImpl { public: + /// The underlying scalar type. using Scalar = TScalar; + /// A complex number is a tuple. static int constexpr kNumParams = 2; + /// Complex multiplication is commutative (and it is a field itself). static bool constexpr kIsCommutative = true; + /// Parameter storage type - to be passed to static functions. using Params = Eigen::Vector; + /// Return type of binary operator + /// + /// In particular relevant mixing different scalars (such as double and + /// ceres::Jet). template using ParamsReturn = Eigen::Vector< typename Eigen::ScalarBinaryOpTraits:: @@ -29,29 +40,39 @@ class ComplexImpl { 2>; // factories + + /// Returns zero constant which is (0, 0). static auto zero() -> Eigen::Vector { return Eigen::Vector::Zero(); } + /// Returns one constant which is (1, 0). static auto one() -> Eigen::Vector { return Eigen::Vector(1.0, 0.0); } + /// There are no particular constraints on the complex space and this function + /// returns always true. static auto areParamsValid(Params const& /*unused*/) -> sophus::Expected { return sophus::Expected{}; } + /// Returns examples of valid parameters. static auto paramsExamples() -> std::vector { return pointExamples(); } + /// Since there are no invalid parameters, this return an empty list. static auto invalidParamsExamples() -> std::vector { return std::vector({}); } // operations + /// Complex addition given two complex numbers (a,i) and (b,j). + /// + /// Returns (a+b, i+j). template static auto addition( Eigen::Vector const& lhs_real_imag, @@ -60,6 +81,9 @@ class ComplexImpl { return lhs_real_imag + rhs_real_imag; } + /// Complex multiplication given two complex numbers (a,i) and (b,j). + /// + /// Returns (a+b-i*j, a*j+b*i). template static auto multiplication( Eigen::Vector const& lhs_real_imag, @@ -73,36 +97,59 @@ class ComplexImpl { lhs_real_imag.y() * rhs_real_imag.x()); } + /// Given input (a,i), return complex conjugate (a, -i). static auto conjugate(Eigen::Vector const& a) -> Eigen::Vector { return Eigen::Vector(a.x(), -a.y()); } + /// Given input (a,i) return complex inverse (a, -i) / (a**2 + i**2). static auto inverse(Eigen::Vector const& real_imag) -> Eigen::Vector { return conjugate(real_imag) / squaredNorm(real_imag); } + /// Given input (a,i), returns complex norm sqrt(a**2 + i**2). static auto norm(Eigen::Vector const& real_imag) -> Scalar { using std::hypot; return hypot(real_imag.x(), real_imag.y()); } + /// Given input (a,i), returns square of complex norm a**2 + i**2. static auto squaredNorm(Eigen::Vector const& real_imag) -> Scalar { return real_imag.squaredNorm(); } }; +/// Complex number class template. +/// +/// A complex number is manifold with additional structure. In particular, +/// it is - like the quaternion numbers - a division ring and fulfills the +/// sophus::concepts::DivisionRingConcept. Furthermore, it is has commutative +/// multiplication (as opposed to the quaternion numbers) and hence is a field. +/// The complex numbers are the only other field of real numbers in addition to +/// the real numbers itself. template class Complex { public: + /// The underlying scalar type. using Scalar = TScalar; + /// Type of the imaginary part is also a Scalar and hence has the some type + /// than the real part. using Imag = Scalar; + /// The stateless implementation. using Impl = ComplexImpl; + + /// A complex number is a tuple. static int constexpr kNumParams = 2; + /// Parameter storage type. using Params = Eigen::Vector; + /// Return type of binary operator + /// + /// In particular relevant mixing different scalars (such as double and + /// ceres::Jet). template using ComplexReturn = Complex:: @@ -110,37 +157,51 @@ class Complex { // constructors and factories + /// Default constructor creates a zero complex number (0, 0). Complex() : params_(Impl::zero()) {} + /// Copy constructor Complex(Complex const&) = default; + /// Copy assignment auto operator=(Complex const&) -> Complex& = default; + /// Returns zero complex number (0, 0). static auto zero() -> Complex { return Complex::fromParams(Impl::zero()); } + /// Returns identity complex number (1, 0). static auto one() -> Complex { return Complex::fromParams(Impl::one()); } + /// Constructs complex number from params tuple. static auto fromParams(Params const& params) -> Complex { Complex z(UninitTag{}); z.setParams(params); return z; } + /// Returns params tuple. [[nodiscard]] auto params() const -> Params const& { return params_; } + /// Set state using params tuple. void setParams(Params const& params) { params_ = params; } + /// Returns reference to real scalar. auto real() -> Scalar& { return params_[0]; } + /// Returns reference to real scalar. [[nodiscard]] auto real() const -> Scalar const& { return params_[0]; } + /// Returns reference to imaginary scalar. auto imag() -> Scalar& { return params_[1]; } + /// Returns reference to imaginary scalar. [[nodiscard]] auto imag() const -> Scalar const& { return params_[1]; } + /// Complex addition template auto operator+(Complex const& other) const -> ComplexReturn { return Complex::fromParams(Impl::addition(this->params_, other.params())); } + /// Complex multiplication template auto operator*(Complex const& other) const -> ComplexReturn { @@ -148,18 +209,22 @@ class Complex { Impl::multiplication(this->params_, other.params())); } + /// Complex conjugate [[nodiscard]] auto conjugate() const -> Complex { return Complex::fromParams(Impl::conjugate(this->params_)); } + /// Complex inverse [[nodiscard]] auto inverse() const -> Complex { return Complex::fromParams(Impl::inverse(this->params_)); } + /// Complex norm [[nodiscard]] auto norm() const -> Scalar { return Impl::norm(this->params_); } + /// Square of complex norm [[nodiscard]] auto squaredNorm() const -> Scalar { return Impl::squaredNorm(this->params_); } diff --git a/cpp/sophus/manifold/product_manifold.h b/cpp/sophus/manifold/product_manifold.h index 9129b2ca..5bd4d1ee 100644 --- a/cpp/sophus/manifold/product_manifold.h +++ b/cpp/sophus/manifold/product_manifold.h @@ -14,40 +14,55 @@ namespace sophus { // Credit: @bogdan at http://stackoverflow.com/q/37373602/6367128 template -constexpr std::array cumulativeSum() { +std::array constexpr cumulativeSum() { int v = 0; return {{v += Ds...}}; } +/// A direct product of manifolds. +/// +/// Note: A product manifold only fulfills the concepts::BaseManifold concept, +/// but not the concepts::Manifold concept. template class ProductManifold { public: + /// Itself. using Self = ProductManifold; + /// Tuple containing sub-manifold. using Tuple = std::tuple; + /// tuple of scalars - one for each sub-manifold using Scalars = std::tuple; - static constexpr size_t kNumManifolds = sizeof...(TSubManifold); - static constexpr std::array kManifoldSizes = { + /// number of sub-manifold + static size_t constexpr kNumManifolds = sizeof...(TSubManifold); + /// Array of DOFs, one for each sub-manifold. + static std::array constexpr kManifoldSizes = { {TSubManifold::kDof...}}; - static constexpr std::array kManifoldStarts = + /// Starting index of each sub-manifold tangent - wrt. the stacked tangent + /// vector of the whole product manifold. + static std::array constexpr kManifoldStarts = cumulativeSum<0, TSubManifold::kDof...>(); + /// scalar type using Scalar = typename std::tuple_element<0, Scalars>::type; - static constexpr size_t kNumParams = [](auto const&... sizes) { + /// total number of parameters, hence the sum over all kNumParams for each + /// sub-manifold. + static size_t constexpr kNumParams = [](auto const&... sizes) { size_t sum = 0; (..., (sum += sizes)); return sum; }(TSubManifold::kNumParams...); - static constexpr size_t kDof = [](auto const&... sizes) { + /// The total DOF, hence the sum over all kDof for each sub-manifold. + static size_t constexpr kDof = [](auto const&... sizes) { size_t sum = 0; (..., (sum += sizes)); return sum; }(TSubManifold::kDof...); - // Return ith diagonal block of matrix representing covariance or similar. + /// Return ith diagonal block of matrix representing covariance or similar. template static auto subBlock(Eigen::MatrixBase const& mat) { return mat @@ -56,27 +71,33 @@ class ProductManifold { .eval(); } + /// Stacked tangent vector of the whole product manifold. using Tangent = Eigen::Vector; ProductManifold() = default; + + /// Copy constructor ProductManifold(ProductManifold const&) = default; + /// Copy assignment ProductManifold& operator=(ProductManifold const&) = default; + /// Construct product manifold by providing all sub-manifolds as args. ProductManifold(TSubManifold const&... manifolds) : manifolds_(manifolds...) {} - // Return ith sub-manifold + /// Return ith sub-manifold template auto& subManifold() { return std::get(manifolds_); } - // Return ith sub-manifold + /// Return ith sub-manifold template auto const& subManifold() const { return std::get(manifolds_); } + /// oplus manifold operator auto oplus(Tangent const& tangent) const -> Self { Self result = *this; oplusImpl( @@ -88,6 +109,7 @@ class ProductManifold { return result; } + /// ominus manifold operator auto ominus(Self const& other) const -> Tangent { Tangent tangent; @@ -101,6 +123,7 @@ class ProductManifold { return tangent; } + /// example of tangent vectors static auto tangentExamples() -> std::vector { std::vector out; Tangent t; @@ -109,6 +132,7 @@ class ProductManifold { return out; } + /// average over a set of input product manifold elements template static std::optional> average( std::array, kArrayLen> const& range) { @@ -169,15 +193,15 @@ class ProductManifold { template static auto getBlock(Tangent& tangent) { - constexpr size_t offset = kManifoldStarts[i]; - constexpr size_t size = kManifoldSizes[i]; + size_t constexpr offset = kManifoldStarts[i]; + size_t constexpr size = kManifoldSizes[i]; return tangent.template segment(offset); } template static auto getBlock(Tangent const& tangent) { - constexpr size_t offset = kManifoldStarts[i]; - constexpr size_t size = kManifoldSizes[i]; + size_t constexpr offset = kManifoldStarts[i]; + size_t constexpr size = kManifoldSizes[i]; return tangent.template segment(offset); } diff --git a/cpp/sophus/manifold/quaternion.h b/cpp/sophus/manifold/quaternion.h index 589ffa14..952e3afc 100644 --- a/cpp/sophus/manifold/quaternion.h +++ b/cpp/sophus/manifold/quaternion.h @@ -13,15 +13,33 @@ namespace sophus { +/// Generic quaternion number implementation +/// +/// Impl class without any storage, but only static methods. +/// +/// Note: This class is following the most common and classic Hamilton +/// representation of quaternions. template class QuaternionImpl { public: + /// The underlying scalar type. using Scalar = TScalar; + /// A quaternion number is a 4-tuple, consisting of 1 real scalar and an + /// imaginary 3-vector. + /// Note: Storage order of the parameters is (imag0, imag1, imag2, real). This + /// is mainly to be compatible with Eigen::Quaternion data layout. static int constexpr kNumParams = 4; + /// Quaternion multiplication is not commutative (and hence the set of of + /// quaternions are not a field but merely a division ring). static bool constexpr kIsCommutative = false; + /// Parameter storage type - to be passed to static functions. using Params = Eigen::Vector; + /// Return type of binary operator + /// + /// In particular relevant mixing different scalars (such as double and + /// ceres::Jet). template using ParamsReturn = Eigen::Vector< typename Eigen::ScalarBinaryOpTraits:: @@ -30,27 +48,47 @@ class QuaternionImpl { // factories + /// Returns zero constant which is (0, 0, 0, 0). static auto zero() -> Eigen::Vector { return Eigen::Vector::Zero(); } + /// Returns one constant which is (0, 0, 0, 1). static auto one() -> Eigen::Vector { return Eigen::Vector(0.0, 0.0, 0.0, 1.0); } + /// There are no particular constraints on the quaternion space and this + /// function returns always true. static auto areParamsValid(Params const& /*unused*/) -> sophus::Expected { return sophus::Expected{}; } + /// Returns examples of valid parameters. static auto paramsExamples() -> std::vector { return pointExamples(); } + /// Since there are no invalid parameters, this return an empty list. static auto invalidParamsExamples() -> std::vector { return std::vector({}); } + /// Quaternion addition given two quaternion numbers (a, v) and (b, u). + /// + /// Returns (a+b, u+v). + template + static auto addition( + Eigen::Vector const& a, + Eigen::Vector const& b) + -> ParamsReturn { + return a + b; + } + + /// Quaternion multiplication given two quaternion numbers (a, v) and (b, u). + /// + /// Returns (a*b - , a*u + b*v + u x v). template static auto multiplication( Eigen::Vector const& lhs, @@ -66,43 +104,56 @@ class QuaternionImpl { return out; } - template - static auto addition( - Eigen::Vector const& a, - Eigen::Vector const& b) - -> ParamsReturn { - return a + b; - } - + /// Given input (a,v), return quaternion conjugate (a, -v). static auto conjugate(Eigen::Vector const& a) -> Eigen::Vector { return Eigen::Vector(-a.x(), -a.y(), -a.z(), a.w()); } + /// Given input (a,i) return quaternion inverse (a, -v) / (a**2 + ). static auto inverse(Eigen::Vector const& q) -> Eigen::Vector { return conjugate(q) / squaredNorm(q); } + /// Given input (a,i), returns quaternion norm sqrt(a**2 + ). static auto norm(Eigen::Vector const& q) -> Scalar { return q.norm(); } + /// Given input (a,i), returns square of quaternion norm a**2 + . static auto squaredNorm(Eigen::Vector const& q) -> Scalar { return q.squaredNorm(); } }; +/// Quaternion number class template. +/// +/// A quaternion number is manifold with additional structure. In particular, +/// it is - like the Quaternion numbers - a division ring and fulfills the +/// sophus::concepts::DivisionRingConcept. template class Quaternion { public: + /// The underlying scalar type. using Scalar = TScalar; + /// Type of the imaginary part is a 3-vector. using Imag = Eigen::Vector; + /// The stateless implementation. using Impl = QuaternionImpl; + /// A quaternion number is a 4-tuple, consisting of 1 real scalar and an + /// imaginary 3-vector. + /// Note: storage order of the parameters is (imag0, imag1, imag2, real). This + /// is mainly to be compatible with Eigen::Quaternion data layout. static int constexpr kNumParams = 4; + /// Parameter storage type. using Params = Eigen::Vector; + /// Return type of binary operator + /// + /// In particular relevant mixing different scalars (such as double and + /// ceres::Jet). template using QuaternionReturn = Quaternion Quaternion& = default; + /// Creates quaternion from params (imag0, imag1, imag2, real) static auto fromParams(Params const& params) -> Quaternion { Quaternion q(UninitTag{}); q.setParams(params); return q; } + /// From real scalar and imaginary 3-vector. + /// + /// Note: Internal storage order is [imag0, imag1, imag2, real]. static auto from(Scalar real, Eigen::Vector const& imag) -> Quaternion { return fromParams(Params(imag[0], imag[1], imag[2], real)); } + /// Returns zero constant which is (0, 0, 0, 0). static auto zero() -> Quaternion { return Quaternion::fromParams(Impl::zero()); } + /// Returns one constant which is (0, 0, 0, 1). static auto one() -> Quaternion { return Quaternion::fromParams(Impl::one()); } + /// Returns params 4-tuple. [[nodiscard]] auto params() const -> Params const& { return params_; } + /// Set state using params 4-tuple. void setParams(Params const& params) { params_ = params; } + /// Returns reference to real scalar. [[nodiscard]] auto real() const -> Scalar const& { return params_[3]; } + + /// Returns reference to real scalar. [[nodiscard]] auto real() -> Scalar& { return params_[3]; } + /// Returns reference to imaginary scalar. [[nodiscard]] auto imag() const { return params_.template head<3>(); } + /// Returns reference to imaginary scalar. [[nodiscard]] auto imag() { return params_.template head<3>(); } + /// Quaternion addition template - [[nodiscard]] auto operator*(Quaternion const& other) const + [[nodiscard]] auto operator+(Quaternion const& other) const -> QuaternionReturn { return QuaternionReturn::fromParams( - Impl::multiplication(this->params_, other.params())); + Impl::addition(this->params_, other.params())); } + /// Quaternion multiplication template - [[nodiscard]] auto operator+(Quaternion const& other) const + [[nodiscard]] auto operator*(Quaternion const& other) const -> QuaternionReturn { return QuaternionReturn::fromParams( - Impl::addition(this->params_, other.params())); + Impl::multiplication(this->params_, other.params())); } + /// Quaternion conjugate [[nodiscard]] auto conjugate() const -> Quaternion { return Quaternion::fromParams(Impl::conjugate(this->params_)); } + /// Quaternion inverse [[nodiscard]] auto inverse() const -> Quaternion { return Quaternion::fromParams(Impl::inverse(this->params_)); } + /// Quaternion norm [[nodiscard]] auto norm() const -> Scalar { return Impl::norm(this->params_); } + /// Square of Quaternion norm [[nodiscard]] auto squaredNorm() const -> Scalar { return Impl::squaredNorm(this->params_); } diff --git a/cpp/sophus/manifold/unit_vector.h b/cpp/sophus/manifold/unit_vector.h index 59afcd6e..c6ad5114 100644 --- a/cpp/sophus/manifold/unit_vector.h +++ b/cpp/sophus/manifold/unit_vector.h @@ -16,7 +16,7 @@ namespace sophus { // Forward declarations -template +template class UnitVector; // Convenience typedefs @@ -29,33 +29,56 @@ using UnitVector2F64 = UnitVector2; using UnitVector3F64 = UnitVector3; namespace linalg { + +/// A n-dimensional vector of unit length. +/// +/// Impl class without any storage, but only static methods. template class UnitVectorImpl { public: + /// scalar type using Scalar = TScalar; + /// The DOF is one dimension smaller than the embedding dimension. + /// Examples: + /// - A unit circle lives in a 2d space, but a point on a circle can be + /// represented using a single scalar (e.g. an angle). + /// - A unit sphere lives in a 3d space, but a point on a sphere can be + /// described using a 2-tuple (e.g. latitude and longitude). static int constexpr kDof = kDim - 1; + /// Internally the unit vector is represented by a vector, hence the number of + /// parameters equals the number of dimension of the embedding space. static int constexpr kNumParams = kDim; + /// internal parameter representation using Params = Eigen::Vector; + /// tangent vector representation using Tangent = Eigen::Vector; + /// return the unit vector along x: (1, 0, 0, ...) static auto unitX() -> UnitVector { return UnitVector::fromUnitVector(Params::UnitX()); } + /// return the unit vector along y: (0, 1, 0, 0, ...) + /// + /// Precondition: kDim >= 1 static auto unitY() -> UnitVector { return UnitVector::fromUnitVector(Params::UnitY()); } + /// return the unit vector along z: (0, 0, 1, 0, 0, ...) + /// + /// Precondition: kDim >= 2 static auto unitZ() -> UnitVector { return UnitVector::fromUnitVector(Params::UnitZ()); } + /// Returns false if the norm of the vector if not close to 1. static auto areParamsValid(Params const& unit_vector) -> sophus::Expected { - static const Scalar kThr = kEpsilon; - const Scalar squared_norm = unit_vector.squaredNorm(); + static Scalar const kThr = kEpsilon; + Scalar const squared_norm = unit_vector.squaredNorm(); using std::abs; if (!(abs(squared_norm - 1.0) <= kThr)) { return SOPHUS_UNEXPECTED( @@ -68,24 +91,32 @@ class UnitVectorImpl { return sophus::Expected{}; } + /// oplus operator + /// + /// See: Hartley Zisserman static auto oplus(Params const& params, Tangent const& delta) -> Params { return matRx(params) * exp(delta); } + /// ominus operator static auto ominus(Params const& lhs_params, Params const& rhs_params) -> Tangent { return log((matRx(lhs_params).transpose() * rhs_params).eval()); } + /// Params examples static auto paramsExamples() -> std::vector { return std::vector( {Params::UnitX(), Params::UnitY(), -Params::UnitX(), -Params::UnitY()}); } + + /// Examples of invalid params. static auto invalidParamsExamples() -> std::vector { return std::vector( {Params::Zero(), Params::Ones(), 2.0 * Params::UnitX()}); } + /// Tangent vector examples static auto tangentExamples() -> std::vector { return std::vector({ Tangent::Zero(), @@ -143,19 +174,37 @@ class UnitVectorImpl { } // namespace linalg -template -class UnitVector : public linalg::UnitVectorImpl { +/// A n-dimensional vector of unit length. +template +class UnitVector : public linalg::UnitVectorImpl { public: + /// scalar type using Scalar = TScalar; - using Impl = linalg::UnitVectorImpl; + + /// The stateless implementation. + using Impl = linalg::UnitVectorImpl; static_assert(concepts::ManifoldImpl); - static int constexpr kDof = kN - 1; - static int constexpr kNumParams = kN; - using Tangent = Eigen::Vector; + /// The DOF is one dimension smaller than the embedding dimension. + /// Examples: + /// - A unit circle lives in a 2d space, but a point on a circle can be + /// represented using a single scalar (e.g. an angle). + /// - A unit sphere lives in a 3d space, but a point on a sphere can be + /// described using a 2-tuple (e.g. latitude and longitude). + static int constexpr kDof = kDim - 1; + /// Internally the unit vector is represented by a vector, hence the number of + /// parameters equals the number of dimension of the embedding space. + static int constexpr kNumParams = kDim; + + /// internal parameter representation using Params = Eigen::Vector; + /// tangent vector representation + using Tangent = Eigen::Vector; - static auto tryFromUnitVector(Eigen::Matrix const& v) + /// Tries to construct it given an input vector v. + /// + /// Returns error if the norm of v is not close to 1 + static auto tryFromUnitVector(Eigen::Matrix const& v) -> Expected { SOPHUS_TRY(auto, maybe_valid, Impl::areParamsValid(v)); UnitVector unit_vector; @@ -163,7 +212,11 @@ class UnitVector : public linalg::UnitVectorImpl { return unit_vector; } - static auto fromParams(Eigen::Matrix const& v) -> UnitVector { + /// Constructs it given an input vector v. + /// + /// Precondition: norm of v must be close to 1 + static auto fromParams(Eigen::Matrix const& v) + -> UnitVector { Expected e_vec = tryFromUnitVector(v); if (!e_vec.has_value()) { SOPHUS_PANIC("{}", e_vec.error()); @@ -171,46 +224,65 @@ class UnitVector : public linalg::UnitVectorImpl { return e_vec.value(); } - // Precondition: v must be of unit length. - static auto fromUnitVector(Eigen::Matrix const& v) + /// Precondition: v must be of unit length. + static auto fromUnitVector(Eigen::Matrix const& v) -> UnitVector { return fromParams(v); } - static auto fromVectorAndNormalize(Eigen::Matrix const& v) + /// Constructs it given an input vector v. + /// + /// If v is not of unit length then it will be normalized accordingly. + static auto fromVectorAndNormalize(Eigen::Matrix const& v) -> UnitVector { return fromUnitVector(v.normalized()); } + /// oplus operator auto oplus(Tangent const& delta) const -> UnitVector { UnitVector v; v.vector_ = Impl::oplus(vector_, delta); return v; } + /// ominus operator auto ominus(UnitVector const& rhs_params) const -> Tangent { return Impl::ominus(vector_, rhs_params.vector_); } - void setParams(Eigen::Matrix const& v) const { + /// Set state using params. + void setParams(Eigen::Matrix const& v) const { SOPHUS_ASSERT(Impl::areParamsValid(v)); vector_.params = v; } - [[nodiscard]] auto params() const -> Eigen::Matrix const& { + /// Get params + [[nodiscard]] auto params() const -> Eigen::Matrix const& { return vector_; } - [[nodiscard]] auto vector() const -> Eigen::Matrix const& { + /// Return as ordinary Eigen vector + [[nodiscard]] auto vector() const -> Eigen::Matrix const& { return vector_; } + /// unsafe mutable access to params pointer + /// + /// Note: It is the users responsibility to hold up the class invariant that + /// the vector must have unit length. [[nodiscard]] auto unsafeMutPtr() { return this->vector_.data(); } + + /// cost access to params pointer [[nodiscard]] auto ptr() const { return this->vector_.data(); } + /// Copy constructor. UnitVector(UnitVector const&) = default; + /// Copy assignment. auto operator=(UnitVector const&) -> UnitVector& = default; + /// Average a set of unit vectors. + /// + /// This is done by summing uo the vectors and re-normalizing the result. template static auto average(TSequenceContainer const& range) -> UnitVector { size_t const len = std::distance(std::begin(range), std::end(range)); @@ -227,7 +299,7 @@ class UnitVector : public linalg::UnitVectorImpl { UnitVector() {} // Class invariant: v_ is of unit length. - Eigen::Matrix vector_; + Eigen::Matrix vector_; }; static_assert(concepts::Manifold>); diff --git a/cpp/sophus/manifold/vector_manifold.h b/cpp/sophus/manifold/vector_manifold.h index 68abd44a..d735f651 100644 --- a/cpp/sophus/manifold/vector_manifold.h +++ b/cpp/sophus/manifold/vector_manifold.h @@ -14,43 +14,60 @@ namespace sophus { +/// A n-dimensional vector is a trivial example of a manifold. +/// template struct VectorManifold { + /// scalar type using Scalar = TScalar; + /// DOF equals kDim static int constexpr kDof = kDim; + /// kNumParams equals kDim static int constexpr kNumParams = kDim; + /// A vector is trivially represented as itself. using Params = Eigen::Vector; + /// The tangent vector is trivially represented as itself. using Tangent = Params; VectorManifold() { vec.setZero(); } + /// Constructs manifold given vector input VectorManifold(Params const& vec) : vec(vec) {} + /// ominus operator is just vector addition auto oplus(Tangent const& a) const -> VectorManifold { return VectorManifold(this->vec + a); } + /// ominus operator is just vector subtraction auto ominus(VectorManifold const& other) const -> Tangent { - return this->vec - other.params(); + this->vec - other.params(); } + /// Construct using params. static auto fromParams(Params const& params) -> VectorManifold { return VectorManifold(params); } + /// Set state using params. auto setParams(Params const& params) -> void { vec = params; } + /// Get params, which is just the vector itself. auto params() const -> Params const& { return vec; } + /// cost access to params pointer auto ptr() const -> Scalar const* { return vec.data(); } + /// unsafe mutable access to params pointer auto unsafeMutPtr() -> Scalar* { return vec.data(); } + /// Examples static auto tangentExamples() -> std::vector { return pointExamples(); } + /// Average a set of vectors. template static auto average(TSequenceContainer const& range) -> std::optional { @@ -64,6 +81,7 @@ struct VectorManifold { return VectorManifold(params / len); } + /// Vector / n-tuple Params vec; }; diff --git a/cpp/sophus/manifold/vector_manifold_test.cpp b/cpp/sophus/manifold/vector_manifold_test.cpp new file mode 100644 index 00000000..a5aeea7f --- /dev/null +++ b/cpp/sophus/manifold/vector_manifold_test.cpp @@ -0,0 +1,19 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include "sophus/manifold/vector_manifold.h" + +#include "sophus/concepts/manifold_prop_tests.h" + +#include + +TEST(vector_manifold, props) { + using Vec = VectorManifold; + + static_assert(concepts::Manifold); +} diff --git a/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error b/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error index 81d19f7e..b320a44e 100644 --- a/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error +++ b/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error @@ -3,7 +3,8 @@ PROJECT_NAME = "farm-ng-core" INPUT = ../../cpp/farm_ng/core \ ../../cpp/sophus/common/ \ ../../cpp/sophus/linalg/ \ - ../../cpp/sophus/calculus/ + ../../cpp/sophus/calculus/ \ + ../../cpp/sophus/manifold FILE_PATTERNS = *.h \ *.hpp \ *.ipp