Introduce Manifold gspline support

[rafaelrojasmiliani]

Motivation

Many times we desire to compute a motion in an arbitrary manifold. For example, a motion in SO(3), a motion in SE(3), in a sphere or a plane.

Scope

This issue ask for the implementation of generalized splines in Manifolds. For example

• Simple computation of gpline on a surface given a sequence of waypoints on such a surface.
• Simple computation of gsplines on SO(3).
• Simple computation of gsplines on SE(3).

What is a GSpline on a Manifold

GSplines are naturally defined in because polynomials an other spaces of functions as as co-domain. This makes a Gspline on depend on the particular basis or chart used to represent the coordinates of the co-domain.

Definition (Gspline on a Manifold Given a Chart) Let be a smooth manifold, be an open set and be a chart. We define a Gspline on given as a map such that there exists a unique Gspline such that .

Remark Note that a Gspline on given may not be a gspline given another chart.

[rafaelrojasmiliani]

• [ ] Expand functions to be able to represent functions from to
• [ ] Create a class of parametrization maps that maps real numbers into custom representation of elements of
• [ ] Crete a class of charts maps that map custom of elements of
• [ ] Define a manifold as a collection of charts
• [ ] An element of of a manifold is a custom representation (e.g. Eigen::Matrix3), a cast contrunstructor and a cast to this custom representation

  class SO3::Element{
  private:
  Eigen::Matrix3d mat_;
  public:
  Element(const Eigen::Matrix3d &_mat);
  operator Eigen::Matrix3d&() { return mat_;}
  }

• [ ] Define an abstract class that represents maps between manifolds

[rafaelrojasmiliani]

How to get first and last type from Variadic Args pack

template <class... Args> struct select_first;
template <class A, class... Args> struct select_first<A, Args...> {
  using type = A;
};
template <typename T> struct tag { using type = T; };

template <typename... Ts> struct select_last {
  // Use a fold-expression to fold the comma operator over the parameter pack.
  using type = typename decltype((tag<Ts>{}, ...))::type;
};

Template Map Composition

template <typename F, typename G> class MapComposition;

template <typename DomainT, typename CodomainT> class Map {
public:
  typedef DomainT Domain;
  typedef CodomainT Codomain;

  virtual Codomain operator()(const Domain &_in) = 0;
};

template <typename F, typename G>
class MapComposition : public Map<typename F::Domain, typename G::Codomain> {
  F f_;
  G g_;

public:
  MapComposition(const F &_f, const G &_g) : f_(_f), g_(_g) {}
  typedef typename F::Domain Domain;
  typedef typename G::Codomain Codomain;
  Codomain operator()(const Domain &_in) { return g_(f_(_in)); }
};

template <typename Current, typename DomainT, typename CodomainT>
class MapInheritanceHelper : public Map<DomainT, CodomainT> {
public:
  template <typename Other>
  MapComposition<Current, Other> operator|(const Other &_in) {
    return MapComposition<Current, Other>(*((Current *)this), _in);
  }
};

template <typename T> class TypeWrapper {
protected:
  T val_;

  TypeWrapper &operator=(const TypeWrapper &_in) = delete;

public:
  typedef T value_type;
  static const uint32_t dim = 1;
  TypeWrapper(T _in) : val_{_in} {}
  TypeWrapper(const TypeWrapper &_in) : val_{_in.val_} {}
  operator const T &() const { return val_; }
  operator T &() { return val_; }

  const T &to_type() const { return val_; }
  T &to_type() { return val_; }
};

typedef TypeWrapper<double> Reals;
typedef TypeWrapper<Eigen::VectorXd> Rn;
typedef TypeWrapper<Eigen::MatrixXd> Hom;

class Sum : public MapInheritanceHelper<Sum, Rn, Reals> {
public:
  Sum(const Sum &) {}
  Sum() {}
  Reals operator()(const Rn &_in) override {
    return ((Eigen::VectorXd)_in).array().sum();
  };
};
class Mat : public MapInheritanceHelper<Mat, Rn, Rn>, Hom {
public:
  Mat(const Mat &_in) : Hom(_in.val_) {}
  Mat(const Hom::value_type &_in) : Hom(_in) {}
  Rn operator()(const Rn &_in) override { return Rn(val_ * _in.to_type()); };
};

int main() {
  Eigen::VectorXd vec(3);
  Eigen::MatrixXd m(3, 3);
  vec << 1, 2, 3;
  m << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  Sum sum;
  Mat mat(m);
  MapComposition<Mat, Sum> p = mat | sum;

  std::cout << p(vec) << "\n";
  return 0;
}

[rafaelrojasmiliani]

Element definition

The elements of a manifold may be defined as a representation wrapper

• Contains an element with its type
• Returns its dimension(i.e. the dimension of the manifold which contains it)

• Can be casted into an element of its type.

  
  template <typename ElementReprentation, long Dim> class Element {
  protected:
  ElementReprentation val_;
  
  Element &operator=(const Element &_in) = delete;

public: typedef ElementReprentation value_type; static const long dim = Dim; static long get_dim() const { return (Dim > 0) ? Dim : 0; } virtual long get_dim_impl() const = 0; Element(ElementReprentation in) : val{_in} {} Element(const Element &in) : val{in.val} {} operator const ElementReprentation &() const { return val; } operator ElementReprentation &() { return val; }

const ElementReprentation &totype() const { return val; } ElementReprentation &totype() { return val; } };