Manifold.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/concepts.h>
 
#include <type_traits>
#include <utility>
 
namespace gtsam {

struct manifold_tag {};

 
template <typename T> struct traits;
 
namespace internal {

template <class C, class ChartJacobian>
using LocalCoordinatesWithJacobians = decltype(
    std::declval<const C&>().localCoordinates(
        std::declval<const C&>(), std::declval<ChartJacobian>(),
        std::declval<ChartJacobian>()));

template <class C, class TangentVector, class ChartJacobian>
using RetractWithJacobians = decltype(std::declval<const C&>().retract(
    std::declval<const TangentVector&>(), std::declval<ChartJacobian>(),
    std::declval<ChartJacobian>()));

template<class Class>
struct HasManifoldPrereqs {
 
  inline constexpr static auto dim = Class::dimension;
 
  Class p, q;
  Eigen::Matrix<double, dim, 1> v;
  OptionalJacobian<dim, dim> Hp, Hq, Hv;
 
  GTSAM_CONCEPT_USAGE(HasManifoldPrereqs) {
    v = p.localCoordinates(q);
    q = p.retract(v);
  }
};

template<class Class, int N>
struct GetDimensionImpl {
  // Get dimension at compile-time for fixed-size manifolds, and at
  // run-time for dynamic-size manifolds.
  static size_t GetDimension(const Class& m) {
    if constexpr (N == Eigen::Dynamic) {
      return m.dim();
    } else {
      return static_cast<size_t>(N);
    }
  }
};

template<class Class>
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
 
  // Check that Class has the necessary machinery
  GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
 
  // Dimension of the manifold
  inline constexpr static auto dimension = Class::dimension;
 
  // Typedefs required by all manifold types.
  typedef Class ManifoldType;
  typedef manifold_tag structure_category;
  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
  // Local coordinates
  static TangentVector Local(const Class& origin, const Class& other) {
    return origin.localCoordinates(other);
  }
 
  template <class C = Class,
            class = LocalCoordinatesWithJacobians<C, ChartJacobian>>
  static TangentVector Local(const Class& origin, const Class& other,
                             ChartJacobian H1, ChartJacobian H2 = {}) {
    return origin.localCoordinates(other, H1, H2);
  }
 
  // Retraction back to manifold
  static Class Retract(const Class& origin, const TangentVector& v) {
    return origin.retract(v);
  }
 
  template <class C = Class,
            class = RetractWithJacobians<C, TangentVector, ChartJacobian>>
  static Class Retract(const Class& origin, const TangentVector& v,
                       ChartJacobian H1, ChartJacobian H2 = {}) {
    return origin.retract(v, H1, H2);
  }
};

template <class T, class = void>
struct HasLocalJacobians : std::false_type {};
 
template <class T>
struct HasLocalJacobians<
    T, std::void_t<decltype(traits<T>::Local(
           std::declval<const T&>(), std::declval<const T&>(),
           std::declval<typename traits<T>::ChartJacobian>(),
           std::declval<typename traits<T>::ChartJacobian>()))>>
    : std::true_type {};

template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
 
} // \ namespace internal

template<typename T>
GTSAM_CONCEPT_REQUIRES(IsTestable<T>, bool) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
  typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
  typename traits<T>::TangentVector v = traits<T>::Local(a,b);
  T c = traits<T>::Retract(a,v);
  return v0.norm() < tol && traits<T>::Equals(b,c,tol);
}

template<typename T>
class IsManifold {
 
public:
 
  using structure_category_tag = typename traits<T>::structure_category;
  static inline constexpr int dim = traits<T>::dimension;
  using ManifoldType = typename traits<T>::ManifoldType;
  using TangentVector = typename traits<T>::TangentVector;
  // Concept marker: allows checking IsManifold<T>::value in templates
  static constexpr bool value =
    std::is_base_of_v<manifold_tag, structure_category_tag>;
 
  GTSAM_CONCEPT_USAGE(IsManifold) {
    static_assert(
        value,
        "This type's structure_category trait does not assert it as a manifold (or derived)");
    if constexpr (dim != Eigen::Dynamic) {
      static_assert(TangentVector::SizeAtCompileTime == dim);
    }
 
    // make sure Chart methods are defined
    v = traits<T>::Local(p, q);
    q = traits<T>::Retract(p, v);
  }
 
private:
 
  TangentVector v;
  ManifoldType p, q;
};

template<typename T>
struct FixedDimension {
  using value_type = const int;
  static inline constexpr int value = traits<T>::dimension;
  static_assert(value != Eigen::Dynamic,
      "FixedDimension instantiated for dynamically-sized type.");
};
} // \ namespace gtsam

// * Macros for using the ManifoldConcept
// *  - An instantiation for use inside unit tests
// *  - A typedef for use inside generic algorithms
// *
// * NOTE: intentionally not in the gtsam namespace to allow for classes not in
// * the gtsam namespace to be more easily enforced as testable
// */
#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;