LieGroup_SO3.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_SO3_H
#define EIGEN_LGSM_LIE_GROUP_SO3_H

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

template<class Derived> 
class LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived> 
{
public:
  /* List of typedef */  
  typedef typename internal::traits<Derived>::Scalar Scalar;
  typedef Quaternion<Scalar> BaseType;
  typedef typename internal::traits<Derived>::Coefficients Coefficients;
  typedef LieGroup<Quaternion<Scalar> > PlainObject;

  typedef Matrix<Scalar, 3, 3> AdjointMatrix;   
  typedef LieAlgebra<Matrix<Scalar, 3, 1> > Algebra;  
  typedef LieAlgebraDual<Matrix<Scalar, 3, 1> > AlgebraDual;

  EIGEN_STRONG_INLINE PlainObject inverse() const;
  static PlainObject Identity();
  template<class OtherDerived> EIGEN_STRONG_INLINE PlainObject operator*(const LieGroupBase<BaseType, OtherDerived>& other) const;
  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*=(const LieGroupBase<BaseType, OtherDerived>& other);
  template<class MatrixDerived> EIGEN_STRONG_INLINE Matrix<Scalar, 3, 1> operator*(const MatrixBase<MatrixDerived>& v) const {
    return this->get() * v;
  }

  inline AdjointMatrix adjoint(void) const;
  template<class AlgebraDerived> inline Algebra adjoint(const LieAlgebraBase<Matrix<Scalar, 3, 1>, AlgebraDerived>& ) const;           // not implemented yet
  template<class AlgebraDualDerived> inline AlgebraDual adjointTr(const LieAlgebraDualBase<Matrix<Scalar, 3, 1>, AlgebraDualDerived>& ) const; // not implemented yet

  Algebra log(const Scalar precision = 1e-6) const;
  
  EIGEN_STRONG_INLINE LieGroupBase& operator=(const LieGroupBase& other);
  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const LieGroupBase<BaseType, OtherDerived>& other);
  template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m){ this->get() = m; return this->derived();}

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


  inline Scalar x() const { return this->get().x(); }
  inline Scalar y() const { return this->get().y(); }
  inline Scalar z() const { return this->get().z(); }
  inline Scalar w() const { return this->get().w(); }

  inline Scalar& x() { return this->get().x(); }
  inline Scalar& y() { return this->get().y(); }
  inline Scalar& z() { return this->get().z(); }
  inline Scalar& w() { return this->get().w(); }

  template<typename NewScalarType>
  inline typename internal::cast_return_type<Derived, LieGroup<Quaternion<NewScalarType> > >::type cast() const
  {
    return typename internal::cast_return_type<Derived, LieGroup<Quaternion<NewScalarType> > >::type(
     get().template cast<NewScalarType>());
  }

  inline const VectorBlock<Coefficients,3> vec() const { return this->get().vec(); }

  inline VectorBlock<Coefficients,3> vec() { return this->get().vec(); }

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

/*****************************************************
 * Implementation of LieGroupBase<Quaternion> methods
 *****************************************************/

// assignation
template<class Derived>
EIGEN_STRONG_INLINE LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>& 
 LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::operator=(const LieGroupBase& other) 
{
  this->get() = other.get();
  return *this;
}

//assignation
template<class Derived>
template<class OtherDerived>
EIGEN_STRONG_INLINE Derived& 
LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::operator=(const LieGroupBase<BaseType, OtherDerived>& other)
{
  this->get() = other.get();
  return derived();
}

// inverse
template<class Derived>
EIGEN_STRONG_INLINE typename LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::PlainObject
 LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::inverse() const 
{
  return PlainObject(this->get().conjugate());
}

// identity
template<class Derived>
typename LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::PlainObject 
 LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::Identity() 
{
  return PlainObject(Quaternion<Scalar>::Identity());
}

// composition
template<class Derived>
template<class OtherDerived> EIGEN_STRONG_INLINE typename LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::PlainObject 
 LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::operator*(const LieGroupBase<Quaternion<Scalar>, OtherDerived>& other) const 
{
  return PlainObject(this->get() * other.get());
}

// composition
template<class Derived>
template<class OtherDerived>
EIGEN_STRONG_INLINE Derived& 
LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::operator*=(const LieGroupBase<BaseType, OtherDerived>& other)
{
  this->get() *= other.get();
  return derived();
}

//log
template<class Derived>
typename  LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::Algebra  
 LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::log(const Scalar precision) const 
{
  const Scalar n2 = this->get().vec().squaredNorm();
  const Scalar n = std::sqrt(n2);

  if (n < precision) 
    return Algebra((2 / this->get().w()) * this->get().vec());
  else 
    return Algebra(std::atan2(2 * n * this->get().w(), this->get().w() * this->get().w() - n2) / n * this->get().vec());
}

// adjoint
template<class Derived>
typename LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::AdjointMatrix 
 LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>::adjoint() const 
{
  return this->get().toRotationMatrix();
}

template<class Derived>
inline std::ostream& operator <<(std::ostream& os, const LieGroupBase<Quaternion<typename internal::traits<Derived>::Scalar>, Derived>& g)
{
  os << g.w() << "\t" << g.x() << "\t" << g.y() << "\t" << g.z();
  return os;
}

/***************************************************************************
* Definition/implementation of LieGroup<quaternion<Scalar> >
***************************************************************************/

template<typename _Scalar> class LieGroup<Quaternion<_Scalar> > : 
  public LieGroupBase<Quaternion<_Scalar>, LieGroup<Quaternion<_Scalar> > > 
{
protected:
  typedef LieGroupBase<Quaternion<_Scalar>, LieGroup<Quaternion<_Scalar> > > Base;
public:
  typedef _Scalar Scalar;
  typedef typename internal::traits<LieGroup<Quaternion<Scalar> > >::Coefficients Coefficients;
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(LieGroup)

  
  inline LieGroup() : m_coeffs() {}
  template<class OtherDerived> inline LieGroup(const LieGroupBase<typename Base::BaseType, OtherDerived>& g) : m_coeffs(g.get()) {}
  //inline LieGroup(const LieGroup& g) : m_coeffs(g.get() ) {}
  EIGEN_STRONG_INLINE LieGroup(const Coefficients& g) : m_coeffs(g) {}
  EIGEN_STRONG_INLINE LieGroup(const AngleAxis<Scalar>& aa) : m_coeffs(aa) {}
  template<class OtherDerived> inline LieGroup(Scalar w, const MatrixBase<OtherDerived>& vec) 
    : m_coeffs(w, vec.coeff(0), vec.coeff(1), vec.coeff(2))
  {
    // [XXX] check other size
  }

  template<typename Derived>
  explicit inline LieGroup(const MatrixBase<Derived> & other) { this->get() = other;}

  template<typename Derived>
  explicit inline LieGroup(const Matrix<Scalar, 4, 1> & other) { this = other;}

  inline LieGroup(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(w, x, y, z) {}
  inline LieGroup(Scalar w, const Matrix<Scalar, 3, 1> & v) : m_coeffs(w, v.x(), v.y(), v.z()) {}

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

protected:
  Coefficients m_coeffs;
};

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

// we don't need anything new

#endif