group.h

Go to the documentation of this file.

#ifndef GCOP_GROUP_H
#define GCOP_GROUP_H

#include <Eigen/Dense>
#include "manifold.h"

namespace gcop {
  
  using namespace Eigen;

  template <int n, int m>class Group : Manifold<Matrix<double, m, m>, n > {
    
    typedef Matrix<double, n, 1> Vectornd;
    typedef Matrix<double, m, m> Matrixmd;
    typedef Matrix<double, n, n> Matrixnd;
    
  public:
    virtual void inv(Matrixmd &gi, const Matrixmd &g) const;

    virtual Matrixmd inv(const Matrixmd &g) const;


    virtual void diff(Matrixmd &dg, const Matrixmd &ga, const Matrixmd &gb) const;


    virtual Matrixmd diff(const Matrixmd &ga, const Matrixmd &gb) const;


    virtual void hat(Matrixmd &g, const Vectornd &v) const = 0;


    virtual Matrixmd hat(const Vectornd &v) const;


    virtual void hatinv(Vectornd &v, const Matrixmd &g) const = 0;


    virtual Vectornd hatinv(const Matrixmd &g) const;


    virtual void Tg(Matrixnd &M, const Matrixmd &g) const = 0;
    

    virtual Matrixnd Tg(const Matrixmd &g) const;

    virtual void Ad(Matrixnd &M, const Matrixmd &g) const = 0;

    virtual Matrixnd Ad(const Matrixmd &g) const;
    

    virtual void ad(Matrixnd &M, const Vectornd &v) const = 0;


    virtual Matrixnd ad(const Vectornd &v) const;
    

    virtual void adinv(Vectornd &v, const Matrixnd &M) const = 0;


    virtual Vectornd adinv(const Matrixnd &M) const;


    virtual void exp(Matrixmd &g, const Vectornd &v) const = 0;


    virtual Matrixmd exp(const Vectornd &v) const;


    virtual void log(Vectornd &v, const Matrixmd &g) const = 0;


    virtual Vectornd log(const Matrixmd &g) const;


    virtual void cay(Matrixmd &g, const Vectornd &v) const;


    virtual Matrixmd cay(const Vectornd &v) const;
 

    virtual void cayinv(Vectornd &v, const Matrixmd &g) const;

    virtual Vectornd cayinv(const Matrixmd &g) const;


    virtual void dcay(Matrixnd &M, const Vectornd &v) const = 0;


    virtual Matrixnd dcay(const Vectornd &v) const;


    virtual void dcayinv(Matrixnd &M, const Vectornd &v) const = 0;


    virtual Matrixnd dcayinv(const Vectornd &v) const;


    virtual void Lift(Vectornd &v,
                      const Matrixmd &ga,
                      const Matrixmd &gb);
    
    
    virtual void Retract(Matrixmd &gb,
                         const Matrixmd &ga,
                         const Vectornd &v);


    virtual void dtau(Matrixnd &M, const Vectornd &v);

    virtual void Adtau(Matrixnd &M, const Vectornd &v);


    static const Matrixmd Id;
    static const Vectornd e;    
  };


  template <int n, int m> 
    Matrix<double, m, m> const Group<n, m>::Id = Matrix<double, m, m>::Identity();
  
  template <int n, int m> 
    Matrix<double, n, 1> const Group<n, m>::e = Matrix<double, n, 1>::Zero();
  
  
  template <int n, int m> 
    void Group<n, m>::inv(Matrix<double, m, m> &gi, const Matrix<double, m, m> &g) const
    {
      // default uses regular matrix inverse
      // methods typically override this
      gi = g.inverse();
    }

  template <int n, int m> 
    Matrix<double, m, m> Group<n, m>::inv(const Matrix<double, m, m> &g) const
    {
      // default uses regular matrix inverse
      // methods typically override this
      Matrix<double, m, m> gi;
      inv(gi, g);
      return gi;
    }
  
  
  template <int n, int m> 
    void Group<n, m>::diff(Matrix<double, m, m> &dg, const Matrix<double, m, m> &ga, const Matrix<double, m, m> &gb) const
    {
      Matrix<double, m, m> gai;
      inv(gai, ga);
      dg = gai*gb;
    }

  template <int n, int m> 
    Matrix<double, m, m> Group<n, m>::diff(const Matrix<double, m, m> &ga, const Matrix<double, m, m> &gb) const
    {
      Matrix<double, m, m> dg;
      diff(dg, ga, gb);
      return dg;
    }
  
  template <int n, int m> 
    Matrix<double, m, m> Group<n, m>::hat(const Vectornd &v) const
    {
      Matrix<double, m, m> g;
      hat(g, v);
      return g;
    }

  template <int n, int m> 
    Matrix<double, n, 1> Group<n, m>::hatinv(const Matrixmd &g) const
    {
      Matrix<double, n, 1> v;
      hatinv(v, g);
      return v;
    }


  template <int n, int m> 
    Matrix<double, n, n> Group<n, m>::Tg(const Matrixmd &g) const
    {
      Matrixnd M;
      Tg(M, g);
      return M;
    }
  

  /*  
  template <int n, int m> 
    void Group<T, n>::exp(Matrix<double, m, m> &g, const MatrixNd &v) const
    {
      // by default only provide a first-order approximation
      // this function is typically overriden
      g = Id + hat(v);
    }
  */

  template <int n, int m> 
    Matrix<double, m, m> Group<n, m>::exp(const Matrix<double, n, 1> &v) const
    {
      Matrix<double, m, m> g;
      exp(g, v);
      return g;
    }


  /*
  template <int n, int m> 
    void Group<T, n>::log(MatrixNd &v, const Matrix<double, m, m> &g) const
    {
      // by default only provide a first-order approximation
      // this function is typically overriden
      v = hatinv(g - Id);
    }
  */

  template <int n, int m> 
    Matrix<double, n, 1> Group<n, m>::log(const Matrix<double, m, m> &g) const
    {
      Matrix<double, n, 1> v;
      log(v, g);
      return v;
    }

  
  template <int n, int m> 
    void Group<n, m>::cay(Matrix<double, m, m> &g, const Matrix<double, n, 1> &v) const
    {
      Matrix<double, m, m> vhh;
      hat(vhh, v/2);
      g = (Id - vhh).inverse()*(Id + vhh);
    }


  template <int n, int m> 
    Matrix<double, m, m> Group<n, m>::cay(const Matrix<double, n, 1> &v) const
    {
      Matrix<double, m, m> vhh;
      hat(vhh, v/2);
      return (Id - vhh).inverse()*(Id + vhh);
    }

  
  template <int n, int m> 
    void Group<n, m>::cayinv(Matrix<double, n, 1> &v, const Matrix<double, m, m> &g) const
    {
      hatinv(v, (Id + g).inverse()*(Id - g));
      v = -2*v;
    }

  template <int n, int m> 
    Matrix<double, n, 1> Group<n, m>::cayinv(const Matrix<double, m, m> &g) const
    {
      return -2*hatinv((Id + g).inverse()*(Id - g));
    }


  template <int n, int m> 
    Matrix<double, n, n> Group<n, m>::ad(const Matrix<double, n, 1> &v) const
    {
      Matrix<double, n, n> M;
      ad(M, v);
      return M;
    }


  template <int n, int m> 
    Matrix<double, n, 1> Group<n, m>::adinv(const Matrix<double, n, n> &M) const
    {
      Matrix<double, n, 1> v;
      adinv(v, M);
      return v;
    }


  template <int n, int m> 
    Matrix<double, n, n> Group<n, m>::Ad(const Matrix<double, m, m> &g) const
    {
      Matrix<double, n, n> M;
      Ad(M, g);
      return M;
    }


  template <int n, int m> 
    Matrix<double, n, n> Group<n, m>::dcay(const Matrix<double, n, 1> &v) const
    {
      Matrix<double, n, n> M;
      dcay(M, v);
      return M;
    }

  template <int n, int m> 
    Matrix<double, n, n> Group<n, m>::dcayinv(const Matrix<double, n, 1> &v) const
    {
      Matrix<double, n, n> M;
      dcayinv(M, v);
      return M;
    }

  template <int n, int m> 
    void Group<n, m>::Lift(Matrix<double, n, 1> &v,
                           const Matrix<double, m, m> &ga,                           
                           const Matrix<double, m, m> &gb) {
    
    Matrix<double, m, m> gai;
    inv(gai, ga);
    log(v, gai*gb);
  }


  template <int n, int m> 
    void Group<n, m>::Retract(Matrix<double, m, m> &gb,
                              const Matrix<double, m, m> &ga,
                              const Matrix<double, n, 1> &v) {
    
    Matrix<double, m, m> g;
    cay(g, v);
    gb = ga*g;
  }

  template <int n, int m> 
    void Group<n, m>::dtau(Matrixnd &M, const Vectornd &v) {
    dcay(M, v);
  }
  
  template <int n, int m> 
    void Group<n, m>::Adtau(Matrixnd &M, const Vectornd &v) {
    Matrix<double, m, m> g;
    cay(g, v);
    Ad(M, g);
  }



}

#endif