LieGroup.h

Go to the documentation of this file.

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009-2013 CEA LIST (DIASI/LSI) <xde-support@saxifrage.cea.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_LGSM_LIE_GROUP_H
#define EIGEN_LGSM_LIE_GROUP_H

/***************************************************************************
* Definition/implementation of LieGroupBase<G, Derived>
***************************************************************************/

namespace internal {
  template<class G, class Derived>
  struct traits<LieGroupBase<G, Derived> > {
    typedef LieGroup<G> PlainObject;
  };
}


template<class G, class Derived> class LieGroupBase {
public:
  /* List of typedef */
  typedef typename Derived::Coefficients Coefficients;
  typedef typename internal::traits<Derived>::Scalar Scalar;
  typedef G BaseType;
  typedef typename internal::traits<Derived>::PlainObject PlainObject;
  typedef typename Derived::AdjointMatrix AdjointMatrix;
  typedef typename Derived::Algebra Algebra;
  typedef typename Derived::CoAlgebra CoAlgebra;

  PlainObject inverse() const;
  static PlainObject Identity();
  template<class OtherDerived> PlainObject operator*(const LieGroupBase<G, OtherDerived>& other) const;

  AdjointMatrix adjoint(void) const;
  Algebra adjoint(const Algebra& ) const;
  CoAlgebra adjointTr(const CoAlgebra& ) const;

  Algebra log(const Scalar precision = 1e-6) const;

  template<class OtherDerived> LieGroupBase& operator=(const LieGroupBase<G, OtherDerived>& );

  inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
  inline Derived& derived() { return *static_cast<Derived*>(this); }

  inline Coefficients& get();
  inline const Coefficients& get() const;
};

/***************************************************************************
* Definition of LieGroup<G>
***************************************************************************/

namespace internal {
  template<class G>
    struct traits<LieGroup<G> > : public traits<LieGroupBase<G, LieGroup<G> > >
    {
      typedef G Coefficients;
      typedef typename G::Scalar Scalar;
    };
}

template<class G> class LieGroup : public LieGroupBase<G, LieGroup<G> > {
protected:
  typedef LieGroupBase<G, LieGroup<G> > Base;
public:
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(LieGroup)

  
  typedef typename internal::traits<LieGroup<G> >::Coefficients Coefficients;

  inline LieGroup(const LieGroup&) {};
  inline LieGroup() {};
    
  inline Coefficients& get() { return m_coeffs; }
  inline const Coefficients& get() const { return m_coeffs; }

protected:
  Coefficients m_coeffs;
};

/***************************************************************************
* Definition/implementation of Map<LieGroup< >
***************************************************************************/


namespace internal {
  template<class G, int MapOptions, typename StrideType>
    struct traits<Map<LieGroup<G>, MapOptions, StrideType> > : public traits<LieGroupBase<G, Map<LieGroup<G>, MapOptions, StrideType> > >
    {
      typedef Map<G, MapOptions, StrideType> Coefficients;
      typedef typename G::Scalar Scalar;
    };

  template<class G, int MapOptions, typename StrideType>
    struct traits<Map<const LieGroup<G>, MapOptions, StrideType> > : public traits<LieGroupBase<G, Map<const LieGroup<G>, MapOptions, StrideType> > >
    {
      typedef Map<const G, MapOptions, StrideType> Coefficients;
      typedef typename G::Scalar Scalar;
    };
}

template<class G, int MapOptions, typename StrideType> class Map<LieGroup<G>, MapOptions, StrideType> : public LieGroupBase<G, Map<LieGroup<G>, MapOptions, StrideType> > {
  protected:
    typedef LieGroupBase<G, Map<LieGroup<G>, MapOptions, StrideType > > Base;
  public:
    // inherit assignement operator
    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
      typedef typename internal::traits<Map<LieGroup<G>, MapOptions, StrideType> >::Scalar Scalar;
    typedef typename internal::traits<Map<LieGroup<G>, MapOptions, StrideType> >::Coefficients Coefficients;

    inline Map(const G& g) : m_coeffs(g) {};
    template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> 
      inline Map(Array<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& g) : m_coeffs(g.data()) {};
    inline Map(Scalar* data) : m_coeffs(data) {};
    inline Map(const Map& m) : m_coeffs(m.get()) {};

    inline Coefficients& get() { return m_coeffs; }
    inline const Coefficients& get() const { return m_coeffs; }

  protected:
    Coefficients m_coeffs;
};

template<class G, int MapOptions, typename StrideType> class Map<const LieGroup<G>, MapOptions, StrideType> : public LieGroupBase<G, Map<const LieGroup<G>, MapOptions, StrideType> > {
  protected:
    typedef LieGroupBase<G, Map<const LieGroup<G>, MapOptions, StrideType > > Base;
  public:
    // inherit assignement operator
    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
      typedef typename internal::traits<Map<const LieGroup<G>, MapOptions, StrideType> >::Scalar Scalar;
    typedef typename internal::traits<Map<const LieGroup<G>, MapOptions, StrideType> >::Coefficients Coefficients;

    inline Map(const G& g) : m_coeffs(g) {};
    template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> 
      inline Map(Array<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& g) : m_coeffs(g.data()) {};
    inline Map(const Scalar* data) : m_coeffs(data) {};
    inline Map(const Map& m) : m_coeffs(m.get()) {};

    inline Coefficients& get() { return m_coeffs; }
    inline const Coefficients& get() const { return m_coeffs; }

  protected:
    Coefficients m_coeffs;
};

#endif