LieAlgebra_se3.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_ALGEBRA_se3_H
#define EIGEN_LGSM_LIE_ALGEBRA_se3_H

/***********************************************************************************
* Definition/implementation of LieAlgebraBase<Matrix<Scalar, 6, 1>, Derived>
************************************************************************************/

template<class Derived> 
class LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> 
  : public MatrixBase<Derived> 
{
protected:
  typedef MatrixBase<Derived> Base;
public:  
  // inherit MatrixBase interface
  EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebraBase)
  // inherit operator= [XXX] inheriting Base::opertor= through warning C4522 with visual studio : multiple assignment operators specified 
  // EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebraBase)
  // accessor needed for MatrixBase inheritance
  LIE_INHERIT_MATRIX_BASE(6, 1)

  typedef Matrix<Scalar, 6, 1> BaseType;
  typedef typename internal::traits<Derived>::Coefficients Coefficients;
  typedef LieAlgebra<BaseType> PlainObject;   
  typedef LieAlgebraDual<BaseType> AlgebraDual;
  typedef typename internal::traits<Derived>::Group Group;
  typedef Matrix<Scalar, 6, 6> Matrix6;

  typedef Matrix<Scalar, 3, 1> Vector3;     
  typedef LieAlgebra<Matrix<Scalar, 3, 1> > so3Element;


  EIGEN_STRONG_INLINE LieAlgebraBase& operator=(const LieAlgebraBase& other);
  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const MatrixBase<OtherDerived>& other);
  
  template<class OtherDerived> PlainObject bracket(const LieAlgebraBase<BaseType, OtherDerived>& a) const;
  inline Group exp(const Scalar precision = 1.e-6) const;
  inline Matrix<Scalar, 6, 6> dexp() const; 
  /*
  inline Map<so3Element> getso3Element(){ return Map<so3Element>(this->get()); }
  inline const Map<so3Element> getso3Element() const {return  Map<so3Element>(this->get()); }
  inline VectorBlock<Derived, 3> getR3Element(){ return this->tail<3>(); }
  inline const VectorBlock<Derived, 3> getR3Element() const { return this->tail<3>(); }*/

  inline Map<so3Element> getso3Element(){ return Map<so3Element>(this->derived().get().template head<3>().data()); }
  inline Map<const so3Element> getso3Element() const {return  Map<const so3Element>(this->derived().get().template head<3>().data()); }
  inline Map<Vector3> getR3Element() { return  Map<Vector3>(this->derived().get().template tail<3>().data()); }
  inline Map<const Vector3> getR3Element() const { return  Map<const Vector3>(this->derived().get().template tail<3>().data()); }

  Coefficients& get() { return derived().get(); }
  const Coefficients& get() const { return derived().get(); }
};

/***************************************************************
 * Implementation of LieAlgebraBase<Array<Scalar, 6, 1> > methods
 ***************************************************************/

// assignation
template<class Derived>
inline LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>& 
  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::operator=(const LieAlgebraBase& other) 
{
  this->get() = other.get();
  return *this;
}

template<class Derived>
template<class OtherDerived>
inline Derived& 
  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::operator=(const MatrixBase<OtherDerived>& other) 
{
  this->get() = other;
  return derived();
}


// bracket 
template<class Derived> 
template<class OtherDerived>
  typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::PlainObject 
  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::bracket(const LieAlgebraBase<Matrix<Scalar, 6, 1>, 
                                                                                                    OtherDerived>& a) const 
{
  return PlainObject(this->getso3Element().cross(a.getso3Element()), 
                     this->getso3Element().cross(a.getR3Element()) - a.getso3Element().cross(this->getR3Element()) );
}
/*
// adjoint
template<class Derived> 
template<class OtherDerived>
  typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived >::PlainObject 
  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived >::adjoint(const LieAlgebraBase<Matrix<Scalar, 6, 1>, 
                                                                                                     OtherDerived>& a) const 
{
  return this->bracket(ang);
}*/

// exp -> unrolling this expression could be 30% faster.
template<class Derived> inline
typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::Group 
  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::exp(const Scalar precision) const 
{
  return Group(this->getso3Element().dexp().transpose() * this->getR3Element()
               ,this->getso3Element().exp());
}

// dexp -> unrolling this expression maybe faster
template<class Derived>
inline typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived >::Matrix6  
  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>::dexp() const 
{ 
  Matrix<Scalar, 6, 6> res;

  // gcc does not seem to like res.block<3,3>(0,0) = this->getso3Element().dexp();
  res.template block<3,3>(0,0) = this->getso3Element().dexp();
  res.template block<3,3>(3,3) = res.template block<3,3>(0,0);
  res.template block<3,3>(0,3).setZero();
  res.template block<3,3>(3,0) = this->getso3Element().d2exp(this->getR3Element());

  return res;
}


/*************************************************************************************
* Definition/implementation of LieAlgebraDualBase<Matrix<Scalar, 6, 1>, Derived>
**************************************************************************************/

template<class Derived> 
class LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> : public MatrixBase<Derived> {
protected:
  typedef MatrixBase<Derived>  Base;
public:
  // inherit MatrixBase interface
  EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebraDualBase)
  // inherit operator= [XXX] inheriting Base::opertor= through warning C4522 with visual studio : multiple assignment operators specified 
  // EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebraDualBase)
  // accessor needed for MatrixBase inheritance
  LIE_INHERIT_MATRIX_BASE(6, 1)

  typedef Matrix<Scalar, 6, 1> BaseType;
  typedef typename internal::traits<Derived>::Coefficients Coefficients;
  typedef LieAlgebraDual<BaseType> PlainObject;   
  typedef LieAlgebra<BaseType> Algebra;

  typedef Matrix<Scalar, 3, 1> Vector3;     
  typedef LieAlgebraDual<Matrix<Scalar, 3, 1> > so3Element;
  
  inline Map<so3Element> getso3Element(){ return Map<so3Element>(this->derived().get().template head<3>().data()); }
  inline Map<const so3Element> getso3Element() const {return  Map<const so3Element>(this->derived().get().template head<3>().data()); }
  inline Map<Vector3> getR3Element() { return  Map<Vector3>(this->derived().get().template tail<3>().data()); }
  inline Map<const Vector3> getR3Element() const { return  Map<const Vector3>(this->derived().get().template tail<3>().data()); }
  
  Coefficients& get() { return derived().get(); }
  const Coefficients& get() const { return derived().get(); }
};

/***************************************************************************
* Definition/implementation of LieAlgebra<Matrix<Scalar, 6, 1> >
***************************************************************************/

namespace internal {
  template<typename Scalar>
    struct traits<LieAlgebra<Matrix<Scalar, 6, 1> > > 
    : public traits<LieAlgebraBase<Matrix<Scalar, 6, 1>, LieAlgebra<Matrix<Scalar, 6, 1> > > >
    {
      typedef Matrix<Scalar, 6, 1> Coefficients;
      typedef LieGroup<Array<Scalar, 7, 1> > Group;
    };

  template<typename Scalar, int Options>
    struct traits<Map<LieAlgebra<Matrix<Scalar, 6, 1> >, Options> > 
    : public traits<LieAlgebraDualBase<Matrix<Scalar, 6, 1>, LieAlgebraDual<Matrix<Scalar, 6, 1> > > >
    {
      typedef Map<Matrix<Scalar, 6, 1>, Options> Coefficients;
      typedef LieGroup<Array<Scalar, 7, 1> > Group;
    };

  template<typename Scalar, int Options>
    struct traits<Map<const LieAlgebra<Matrix<Scalar, 6, 1> >, Options> > 
    : public traits<LieAlgebraDualBase<Matrix<Scalar, 6, 1>, LieAlgebraDual<Matrix<Scalar, 6, 1> > > >
    {
      typedef Map<const Matrix<Scalar, 6, 1>, Options> Coefficients;
      typedef LieGroup<Array<Scalar, 7, 1> > Group;
    };

}

template<typename _Scalar> class LieAlgebra<Matrix<_Scalar, 6, 1> > : 
  public LieAlgebraBase<Matrix<_Scalar, 6, 1>, LieAlgebra<Matrix<_Scalar, 6, 1> > > 
{
protected:
  typedef LieAlgebraBase<Matrix<_Scalar, 6, 1>, LieAlgebra<Matrix<_Scalar, 6, 1> > > Base;
public:
  // inherit MatrixBase operator
  EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebra)
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebra)

  typedef typename Base::BaseType BaseType;
  typedef typename internal::traits<LieAlgebra<Matrix<Scalar, 6, 1> > >::Coefficients Coefficients;

  inline LieAlgebra() : m_coeffs() {}
  inline LieAlgebra(const LieAlgebra& g) : m_coeffs(g.get() ) {}
  inline LieAlgebra(const BaseType& g) : m_coeffs(g) {}
  inline LieAlgebra(Scalar rx, Scalar ry, Scalar rz, Scalar vx, Scalar vy, Scalar vz) {
    m_coeffs[0] = rx;
    m_coeffs[1] = ry;
    m_coeffs[2] = rz;
    m_coeffs[3] = vx;
    m_coeffs[4] = vy;
    m_coeffs[5] = vz;
  }
  inline LieAlgebra(const typename Base::so3Element& r, const typename Base::Vector3& v) {
    this->getR3Element() = v;
    this->getso3Element() = r;
  }

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

protected:
  Coefficients m_coeffs;
};

/***************************************************************************
* Definition/implementation of LieAlgebraDual<Matrix<Scalar, 6, 1> >
***************************************************************************/

template<typename _Scalar> class LieAlgebraDual<Matrix<_Scalar, 6, 1> > : 
  public LieAlgebraDualBase<Matrix<_Scalar, 6, 1>, LieAlgebraDual<Matrix<_Scalar, 6, 1> > > 
{
protected:
  typedef LieAlgebraDualBase<Matrix<_Scalar, 6, 1>, LieAlgebraDual<Matrix<_Scalar, 6, 1> > > Base;
public:
  // inherit MatrixBase operator
  EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebraDual)
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebraDual)

  typedef typename internal::traits<LieAlgebraDual<Matrix<Scalar, 6, 1> > >::Coefficients Coefficients;

  inline LieAlgebraDual() : m_coeffs() {}
  inline LieAlgebraDual(const LieAlgebraDual& g) : m_coeffs(g.get() ) {}
  inline LieAlgebraDual(const Matrix<Scalar, 6, 1>& g) : m_coeffs(g) {}
  inline LieAlgebraDual(Scalar rx, Scalar ry, Scalar rz, Scalar vx, Scalar vy, Scalar vz) {
    m_coeffs << rx, ry, rz, vx, vy, vz;
  }
  inline LieAlgebraDual(const typename Base::so3Element& r, const typename Base::Vector3& v) {
    this->getR3Element() = v;
    this->getso3Element() = r;
  }

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

protected:
  Coefficients m_coeffs;
};

#endif