SO3_Group.hpp

#ifndef DEF_COMMON_SO3_GROUP
#define DEF_COMMON_SO3_GROUP

#include <string>

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace SO3
{

template <typename T_SCALAR_TYPE>
class Group : public LieGroupBase<Group<T_SCALAR_TYPE>,3>
{
protected:
    Eigen::Quaternion<T_SCALAR_TYPE> m_q;

//public:
    //static const unsigned int DOF = 3;

public:
    Group<T_SCALAR_TYPE> ( )
    : m_q(Eigen::Quaternion<T_SCALAR_TYPE>::Identity())
    { }

    Group<T_SCALAR_TYPE> (const Group<T_SCALAR_TYPE>& g)
    : m_q(g.m_q)
    { }

    Group<T_SCALAR_TYPE> (const Eigen::Quaternion<T_SCALAR_TYPE>& q_)
    { q(q_); }

    Group<T_SCALAR_TYPE> (  const T_SCALAR_TYPE& w,
                            const T_SCALAR_TYPE& x,
                            const T_SCALAR_TYPE& y,
                            const T_SCALAR_TYPE& z)
    { q(w,x,y,z); }

    Group<T_SCALAR_TYPE> (const T_SCALAR_TYPE* data)
    { q(data); }

    template<class Derived>
    Group<T_SCALAR_TYPE> (const Eigen::QuaternionBase<Derived>& other)
    { q(other); }

    Group<T_SCALAR_TYPE> (const Eigen::AngleAxis<T_SCALAR_TYPE>& aa)
    { q(aa); }

    template<typename Derived>
    Group<T_SCALAR_TYPE> (const Eigen::MatrixBase<Derived>& other)
    { q(other); }

    template<class OtherScalar, int OtherOptions>
    Group<T_SCALAR_TYPE> (const Eigen::Quaternion<OtherScalar,OtherOptions>& other)
    { q(other); }

    /* Accessors and setters */

    Eigen::Quaternion<T_SCALAR_TYPE>
    q ( ) const
    { return m_q; }

    void
    q (const Eigen::Quaternion<T_SCALAR_TYPE>& q_)
    { m_q = q_; m_q.normalize(); }

    // see Eigen::Quaternion constructors
    void
    q ( const T_SCALAR_TYPE& w,
        const T_SCALAR_TYPE& x,
        const T_SCALAR_TYPE& y,
        const T_SCALAR_TYPE& z)
    { m_q = Eigen::Quaternion<T_SCALAR_TYPE>(w,x,y,z); m_q.normalize(); }

    // see Eigen::Quaternion constructors
    void
    q (const T_SCALAR_TYPE* data)
    { m_q = Eigen::Quaternion<T_SCALAR_TYPE>(data); m_q.normalize(); }

    // see Eigen::Quaternion constructors
    template<class Derived>
    void
    q (const Eigen::QuaternionBase<Derived>& other)
    { m_q = Eigen::Quaternion<T_SCALAR_TYPE>(other); m_q.normalize(); }

    // see Eigen::Quaternion constructors
    void
    q (const Eigen::AngleAxis<T_SCALAR_TYPE>& aa)
    { m_q = Eigen::Quaternion<T_SCALAR_TYPE>(aa); m_q.normalize(); }
    
    // see Eigen::Quaternion constructors
    template<typename Derived>
    void
    q (const Eigen::MatrixBase<Derived>& other)
    { m_q = Eigen::Quaternion<T_SCALAR_TYPE>(other); m_q.normalize(); }

    // see Eigen::Quaternion constructors
    template<class OtherScalar, int OtherOptions>
    void
    q (const Eigen::Quaternion<OtherScalar,OtherOptions>& other)
    { m_q = Eigen::Quaternion<T_SCALAR_TYPE>(other); m_q.normalize(); }

    /* Group operations */

    Group<T_SCALAR_TYPE>
    inverse( ) const
    { return Group<T_SCALAR_TYPE>(m_q.inverse()); }

    static Group<T_SCALAR_TYPE>
    Identity ( )
    { return Group<T_SCALAR_TYPE>(Eigen::Quaternion<T_SCALAR_TYPE>::Identity()); }

    /* Group operation is '*' and not '+' since we are used to the matrix representation,
     * in which case the mutliplication is the group operation */
    void
    operator*= (Group<T_SCALAR_TYPE> const& g)
    { m_q *= g.q(); m_q.normalize(); }

    Group<T_SCALAR_TYPE>
    operator* (Group<T_SCALAR_TYPE> const& g) const
    {
        Group<T_SCALAR_TYPE> res(*this);
        res *= g;
        return res;
    }

    Eigen::Matrix<T_SCALAR_TYPE,3,1>
    transformVector (const Eigen::Matrix<T_SCALAR_TYPE,3,1>& v) const
    { return this->toRotationMatrix()*v; }

    Eigen::Matrix<T_SCALAR_TYPE,3,1>
    operator* (Eigen::Matrix<T_SCALAR_TYPE,3,1> const& v) const
    { return m_q._transformVector(v); }

    Eigen::Matrix<T_SCALAR_TYPE,3,3>
    toRotationMatrix ( ) const
    { return m_q.toRotationMatrix(); }

    Eigen::Matrix<T_SCALAR_TYPE,3,3>
    toAxisAngle ( ) const
    { return Eigen::AngleAxis<T_SCALAR_TYPE>(m_q); }

    Eigen::Matrix<T_SCALAR_TYPE,3,1>
    toVector ( ) const
    {
        Eigen::AngleAxis<T_SCALAR_TYPE> aa(m_q);
        return aa.angle()*aa.axis();
    }

    bool
    isApprox (Group<T_SCALAR_TYPE> const& g) const
    { return m_q.isApprox(g.q()); }

};

} // namespace SO3

template <typename T_SCALAR_TYPE>
std::ostream
operator<< (std::ostream& stream, SO3::Group<T_SCALAR_TYPE> const& G)
{
    stream << G.toRotationMatrix();
    return stream;
}

#endif