GAAS

// This file is part of Sophus.
//
// Copyright 2011-2013 Hauke Strasdat
// Copyrifht 2012-2013 Steven Lovegrove
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#ifndef SOPHUS_SO3_HPP
#define SOPHUS_SO3_HPP

#include "sophus.hpp"

////////////////////////////////////////////////////////////////////////////
// Forward Declarations / typedefs
////////////////////////////////////////////////////////////////////////////

namespace Sophus {
    template<typename _Scalar, int _Options = 0>
    class SO3Group;

    typedef EIGEN_DEPRECATED SO3Group<double> SO3;
    typedef SO3Group<double> SO3d; /**< double precision SO3 */
    typedef SO3Group<float> SO3f;  /**< single precision SO3 */
}

////////////////////////////////////////////////////////////////////////////
// Eigen Traits (For querying derived types in CRTP hierarchy)
////////////////////////////////////////////////////////////////////////////

namespace Eigen {
    namespace internal {

        template<typename _Scalar, int _Options>
        struct traits<Sophus::SO3Group<_Scalar, _Options> > {
            typedef _Scalar Scalar;
            typedef Quaternion<Scalar> QuaternionType;
        };

        template<typename _Scalar, int _Options>
        struct traits<Map<Sophus::SO3Group<_Scalar>, _Options> >
                : traits<Sophus::SO3Group<_Scalar, _Options> > {
            typedef _Scalar Scalar;
            typedef Map<Quaternion<Scalar>, _Options> QuaternionType;
        };

        template<typename _Scalar, int _Options>
        struct traits<Map<const Sophus::SO3Group<_Scalar>, _Options> >
                : traits<const Sophus::SO3Group<_Scalar, _Options> > {
            typedef _Scalar Scalar;
            typedef Map<const Quaternion<Scalar>, _Options> QuaternionType;
        };

    }
}

namespace Sophus {
    using namespace Eigen;

/**
 * \brief SO3 base type - implements SO3 class but is storage agnostic
 *
 * [add more detailed description/tutorial]
 */
    template<typename Derived>
    class SO3GroupBase {
    public:
        /** \brief scalar type */
        typedef typename internal::traits<Derived>::Scalar Scalar;
        /** \brief quaternion reference type  */
        typedef typename internal::traits<Derived>::QuaternionType &
                QuaternionReference;
        /** \brief quaternion const reference type  */
        typedef const typename internal::traits<Derived>::QuaternionType &
                ConstQuaternionReference;

        /** \brief degree of freedom of group
         *         (three for rotation) */
        static const int DoF = 3;
        /** \brief number of internal parameters used
         *         (unit quaternion for rotation) */
        static const int num_parameters = 4;
        /** \brief group transformations are NxN matrices */
        static const int N = 3;
        /** \brief group transfomation type */
        typedef Matrix<Scalar, N, N> Transformation;
        /** \brief point type */
        typedef Matrix<Scalar, 3, 1> Point;
        /** \brief tangent vector type */
        typedef Matrix<Scalar, DoF, 1> Tangent;
        /** \brief adjoint transformation type */
        typedef Matrix<Scalar, DoF, DoF> Adjoint;


        /**
         * \brief Jr, right jacobian of SO(3)
         */
        inline static
        Transformation JacobianR(const Tangent &w) {
            Transformation Jr = Transformation::Identity();
            Scalar theta = w.norm();
            if (theta < SophusConstants<Scalar>::epsilon()) {
                return Jr;
            } else {
                Tangent k = w.normalized();  // k - unit direction vector of w
                Transformation K = hat(k);
                Jr = Jr - (1 - std::cos(theta)) / theta * K + (1 - std::sin(theta) / theta) * K * K;
                return Jr;
            }
        }

        /**
         * \brief JrInv, inverse of right Jacobian
         */
        inline static Transformation JacobianRInv(const Tangent &w) {
            Transformation Jrinv = Transformation::Identity();
            Scalar theta = w.norm();
            if (theta < SophusConstants<Scalar>::epsilon()) {
                return Jrinv;
            } else {
                Tangent k = w.normalized();  // k - unit direction vector of w
                Transformation K = hat(k);
                Jrinv = Jrinv
                        + 0.5 * hat(k)
                        + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K;
            }

            return Jrinv;
        }

        // left jacobian in SO(3)
        inline static Transformation JacobianL(const Tangent &w) {
            return JacobianR(-w);
        }

        // left inverse jacobian in SO(3)
        inline static Transformation JacobianLInv(const Tangent &w) {
            return JacobianRInv(-w);
        }

        /**
         * \brief Adjoint transformation
         *
         * This function return the adjoint transformation \f$ Ad \f$ of the
         * group instance \f$ A \f$  such that for all \f$ x \f$
         * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
         * with \f$\ \widehat{\cdot} \f$ being the hat()-Vector4Scalaror.
         *
         * For SO3, it simply returns the rotation matrix corresponding to \f$ A \f$.
         */
        inline
        const Adjoint Adj() const {
            return matrix();
        }

        /**
         * \returns copy of instance casted to NewScalarType
         */
        template<typename NewScalarType>
        inline SO3Group<NewScalarType> cast() const {
            return SO3Group<NewScalarType>(unit_quaternion()
                                                   .template cast<NewScalarType>());
        }

        /**
         * \returns pointer to internal data
         *
         * This provides unsafe read/write access to internal data. SO3 is represented
         * by an Eigen::Quaternion (four parameters). When using direct write access,
         * the user needs to take care of that the quaternion stays normalized.
         *
         * Note: The first three Scalars represent the imaginary parts, while the
         * forth Scalar represent the real part.
         *
         * \see normalize()
         */
        inline Scalar *data() {
            return unit_quaternion_nonconst().coeffs().data();
        }

        /**
         * \returns const pointer to internal data
         *
         * Const version of data().
         */
        inline const Scalar *data() const {
            return unit_quaternion().coeffs().data();
        }

        /**
         * \brief Fast group multiplication
         *
         * This method is a fast version of operator*=(), since it does not perform
         * normalization. It is up to the user to call normalize() once in a while.
         *
         * \see operator*=()
         */
        inline
        void fastMultiply(const SO3Group<Scalar> &other) {
            unit_quaternion_nonconst() *= other.unit_quaternion();
        }

        /**
         * \returns group inverse of instance
         */
        inline
        const SO3Group<Scalar> inverse() const {
            return SO3Group<Scalar>(unit_quaternion().conjugate());
        }

        /**
         * \brief Logarithmic map
         *
         * \returns tangent space representation (=rotation vector) of instance
         *
         * \see  log().
         */
        inline
        const Tangent log() const {
            return SO3Group<Scalar>::log(*this);
        }

        /**
         * \brief Normalize quaternion
         *
         * It re-normalizes unit_quaternion to unit length. This method only needs to
         * be called in conjunction with fastMultiply() or data() write access.
         */
        inline
        void normalize() {
            Scalar length = unit_quaternion_nonconst().norm();
            if (length < SophusConstants<Scalar>::epsilon()) {
                throw SophusException("Quaternion is (near) zero!");
            }
            unit_quaternion_nonconst().coeffs() /= length;
        }

        /**
         * \returns 3x3 matrix representation of instance
         *
         * For SO3, the matrix representation is an orthogonal matrix R with det(R)=1,
         * thus the so-called rotation matrix.
         */
        inline
        const Transformation matrix() const {
            return unit_quaternion().toRotationMatrix();
        }

        /**
         * \brief Assignment operator
         */
        template<typename OtherDerived>
        inline
        SO3GroupBase<Derived> &operator=(const SO3GroupBase<OtherDerived> &other) {
            unit_quaternion_nonconst() = other.unit_quaternion();
            return *this;
        }

        /**
         * \brief Group multiplication
         * \see operator*=()
         */
        inline
        const SO3Group<Scalar> operator*(const SO3Group<Scalar> &other) const {
            SO3Group<Scalar> result(*this);
            result *= other;
            return result;
        }

        /**
         * \brief Group action on \f$ \mathbf{R}^3 \f$
         *
         * \param p point \f$p \in \mathbf{R}^3 \f$
         * \returns point \f$p' \in \mathbf{R}^3 \f$, rotated version of \f$p\f$
         *
         * This function rotates a point \f$ p \f$ in  \f$ \mathbf{R}^3 \f$ by the
         * SO3 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$.
         *
         *
         * Since SO3 is intenally represented by a unit quaternion \f$q\f$, it is
         * implemented as \f$ p' = q\cdot p \cdot q^{*} \f$
         * with \f$ q^{*} \f$ being the quaternion conjugate of \f$ q \f$.
         *
         * Geometrically, \f$ p \f$  is rotated by angle \f$|\omega|\f$ around the
         * axis \f$\frac{\omega}{|\omega|}\f$ with \f$ \omega = \log(R)^\vee \f$.
         *
         * \see log()
         */
        inline
        const Point operator*(const Point &p) const {
            return unit_quaternion()._transformVector(p);
        }

        /**
         * \brief In-place group multiplication
         *
         * \see fastMultiply()
         * \see operator*()
         */
        inline
        void operator*=(const SO3Group<Scalar> &other) {
            fastMultiply(other);
            normalize();
        }

        /**
         * \brief Setter of internal unit quaternion representation
         *
         * \param quaternion
         * \pre   the quaternion must not be zero
         *
         * The quaternion is normalized to unit length.
         */
        inline
        void setQuaternion(const Quaternion<Scalar> &quaternion) {
            unit_quaternion_nonconst() = quaternion;
            normalize();
        }

        /**
         * \brief Accessor of unit quaternion
         *
         * No direct write access is given to ensure the quaternion stays normalized.
         */
        EIGEN_STRONG_INLINE
        ConstQuaternionReference unit_quaternion() const {
            return static_cast<const Derived *> ( this )->unit_quaternion();
        }

        ////////////////////////////////////////////////////////////////////////////
        // public static functions
        ////////////////////////////////////////////////////////////////////////////

        /**
         * \param   b 3-vector representation of Lie algebra element
         * \returns   derivative of Lie bracket
         *
         * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{so3} \f$
         * with \f$ [a, b]_{so3} \f$ being the lieBracket() of the Lie algebra so3.
         *
         * \see lieBracket()
         */
        inline static
        const Adjoint d_lieBracketab_by_d_a(const Tangent &b) {
            return -hat(b);
        }

        /**
         * \brief Group exponential
         *
         * \param omega tangent space element (=rotation vector \f$ \omega \f$)
         * \returns     corresponding element of the group SO3
         *
         * To be more specific, this function computes \f$ \exp(\widehat{\omega}) \f$
         * with \f$ \exp(\cdot) \f$ being the matrix exponential
         * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO3.
         *
         * \see expAndTheta()
         * \see hat()
         * \see log()
         */
        inline static
        const SO3Group<Scalar> exp(const Tangent &omega) {
            Scalar theta;
            return expAndTheta(omega, &theta);
        }

        /**
         * \brief Group exponential and theta
         *
         * \param      omega tangent space element (=rotation vector \f$ \omega \f$)
         * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
         * \returns          corresponding element of the group SO3
         *
         * \see exp() for details
         */
        inline static
        const SO3Group<Scalar> expAndTheta(const Tangent &omega,
                                           Scalar *theta) {
            const Scalar theta_sq = omega.squaredNorm();
            *theta = std::sqrt(theta_sq);
            const Scalar half_theta = static_cast<Scalar> ( 0.5 ) * (*theta);

            Scalar imag_factor;
            Scalar real_factor;;
            if ((*theta) < SophusConstants<Scalar>::epsilon()) {
                const Scalar theta_po4 = theta_sq * theta_sq;
                imag_factor = static_cast<Scalar> ( 0.5 )
                              - static_cast<Scalar> ( 1.0 / 48.0 ) * theta_sq
                              + static_cast<Scalar> ( 1.0 / 3840.0 ) * theta_po4;
                real_factor = static_cast<Scalar> ( 1 )
                              - static_cast<Scalar> ( 0.5 ) * theta_sq +
                              static_cast<Scalar> ( 1.0 / 384.0 ) * theta_po4;
            } else {
                const Scalar sin_half_theta = std::sin(half_theta);
                imag_factor = sin_half_theta / (*theta);
                real_factor = std::cos(half_theta);
            }

            return SO3Group<Scalar>(Quaternion<Scalar>(real_factor,
                                                       imag_factor * omega.x(),
                                                       imag_factor * omega.y(),
                                                       imag_factor * omega.z()));
        }

        /**
         * \brief Generators
         *
         * \pre \f$ i \in \{0,1,2\} \f$
         * \returns \f$ i \f$th generator \f$ G_i \f$ of SO3
         *
         * The infinitesimal generators of SO3
         * are \f$
         *        G_0 = \left( \begin{array}{ccc}
         *                          0&  0&  0& \\
         *                          0&  0& -1& \\
         *                          0&  1&  0&
         *                     \end{array} \right),
         *        G_1 = \left( \begin{array}{ccc}
         *                          0&  0&  1& \\
         *                          0&  0&  0& \\
         *                         -1&  0&  0&
         *                     \end{array} \right),
         *        G_2 = \left( \begin{array}{ccc}
         *                          0& -1&  0& \\
         *                          1&  0&  0& \\
         *                          0&  0&  0&
         *                     \end{array} \right).
         * \f$
         * \see hat()
         */
        inline static
        const Transformation generator(int i) {
            if (i < 0 || i > 2) {
                throw SophusException("i is not in range [0,2].");
            }
            Tangent e;
            e.setZero();
            e[i] = static_cast<Scalar> ( 1 );
            return hat(e);
        }

        /**
         * \brief hat-operator
         *
         * \param omega 3-vector representation of Lie algebra element
         * \returns     3x3-matrix representatin of Lie algebra element
         *
         * Formally, the hat-operator of SO3 is defined
         * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{3\times 3},
         * \quad \widehat{\omega} = \sum_{i=0}^2 G_i \omega_i \f$
         * with \f$ G_i \f$ being the ith infinitesial generator().
         *
         * \see generator()
         * \see vee()
         */
        inline static
        const Transformation hat(const Tangent &omega) {
            Transformation Omega;
            Omega << static_cast<Scalar> ( 0 ), -omega(2), omega(1)
                    , omega(2), static_cast<Scalar> ( 0 ), -omega(0)
                    , -omega(1), omega(0), static_cast<Scalar> ( 0 );
            return Omega;
        }

        /**
         * \brief Lie bracket
         *
         * \param omega1 3-vector representation of Lie algebra element
         * \param omega2 3-vector representation of Lie algebra element
         * \returns      3-vector representation of Lie algebra element
         *
         * It computes the bracket of SO3. To be more specific, it
         * computes \f$ [\omega_1, \omega_2]_{so3}
         * := [\widehat{\omega_1}, \widehat{\omega_2}]^\vee \f$
         * with \f$ [A,B] = AB-BA \f$ being the matrix
         * commutator, \f$ \widehat{\cdot} \f$ the
         * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SO3.
         *
         * For the Lie algebra so3, the Lie bracket is simply the
         * cross product: \f$ [\omega_1, \omega_2]_{so3}
         *                    = \omega_1 \times \omega_2 \f$.
         *
         * \see hat()
         * \see vee()
         */
        inline static
        const Tangent lieBracket(const Tangent &omega1,
                                 const Tangent &omega2) {
            return omega1.cross(omega2);
        }

        /**
         * \brief Logarithmic map
         *
         * \param other element of the group SO3
         * \returns     corresponding tangent space element
         *              (=rotation vector \f$ \omega \f$)
         *
         * Computes the logarithmic, the inverse of the group exponential.
         * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
         * with \f$ \vee(\cdot) \f$ being the matrix logarithm
         * and \f$ \vee{\cdot} \f$ the vee()-operator of SO3.
         *
         * \see exp()
         * \see logAndTheta()
         * \see vee()
         */
        inline static
        const Tangent log(const SO3Group<Scalar> &other) {
            Scalar theta;
            return logAndTheta(other, &theta);
        }

        /**
         * \brief Logarithmic map and theta
         *
         * \param      other element of the group SO3
         * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
         * \returns          corresponding tangent space element
         *                   (=rotation vector \f$ \omega \f$)
         *
         * \see log() for details
         */
        inline static
        const Tangent logAndTheta(const SO3Group<Scalar> &other,
                                  Scalar *theta) {
            const Scalar squared_n
                    = other.unit_quaternion().vec().squaredNorm();
            const Scalar n = std::sqrt(squared_n);
            const Scalar w = other.unit_quaternion().w();

            Scalar two_atan_nbyw_by_n;

            // Atan-based log thanks to
            //
            // C. Hertzberg et al.:
            // "Integrating Generic Sensor Fusion Algorithms with Sound State
            // Representation through Encapsulation of Manifolds"
            // Information Fusion, 2011

            if (n < SophusConstants<Scalar>::epsilon()) {
                // If quaternion is normalized and n=0, then w should be 1;
                // w=0 should never happen here!
                if (std::abs(w) < SophusConstants<Scalar>::epsilon()) {
                    throw SophusException("Quaternion is not normalized!");
                }
                const Scalar squared_w = w * w;
                two_atan_nbyw_by_n = static_cast<Scalar> ( 2 ) / w
                                     - static_cast<Scalar> ( 2 ) * (squared_n) / (w * squared_w);
            } else {
                if (std::abs(w) < SophusConstants<Scalar>::epsilon()) {
                    if (w > static_cast<Scalar> ( 0 )) {
                        two_atan_nbyw_by_n = M_PI / n;
                    } else {
                        two_atan_nbyw_by_n = -M_PI / n;
                    }
                } else {
                    two_atan_nbyw_by_n = static_cast<Scalar> ( 2 ) * atan(n / w) / n;
                }
            }

            *theta = two_atan_nbyw_by_n * n;

            return two_atan_nbyw_by_n * other.unit_quaternion().vec();
        }

        /**
         * \brief vee-operator
         *
         * \param Omega 3x3-matrix representation of Lie algebra element
         * \pr          Omega must be a skew-symmetric matrix
         * \returns     3-vector representatin of Lie algebra element
         *
         * This is the inverse of the hat()-operator.
         *
         * \see hat()
         */
        inline static
        const Tangent vee(const Transformation &Omega) {
            return static_cast<Scalar> ( 0.5 ) * Tangent(Omega(2, 1) - Omega(1, 2),
                                                         Omega(0, 2) - Omega(2, 0),
                                                         Omega(1, 0) - Omega(0, 1));
        }


    private:
        // Mutator of unit_quaternion is private so users are hampered
        // from setting non-unit quaternions.
        EIGEN_STRONG_INLINE
        QuaternionReference unit_quaternion_nonconst() {
            return static_cast<Derived *> ( this )->unit_quaternion_nonconst();
        }

    };

/**
 * \brief SO3 default type - Constructors and default storage for SO3 Type
 */
    template<typename _Scalar, int _Options>
    class SO3Group : public SO3GroupBase<SO3Group<_Scalar, _Options> > {
        typedef SO3GroupBase<SO3Group<_Scalar, _Options> > Base;
    public:
        /** \brief scalar type */
        typedef typename internal::traits<SO3Group<_Scalar, _Options> >
        ::Scalar Scalar;
        /** \brief quaternion type */
        typedef typename internal::traits<SO3Group<_Scalar, _Options> >
        ::QuaternionType &QuaternionReference;
        typedef const typename internal::traits<SO3Group<_Scalar, _Options> >
        ::QuaternionType &ConstQuaternionReference;

        /** \brief degree of freedom of group */
        static const int DoF = Base::DoF;
        /** \brief number of internal parameters used */
        static const int num_parameters = Base::num_parameters;
        /** \brief group transformations are NxN matrices */
        static const int N = Base::N;
        /** \brief group transfomation type */
        typedef typename Base::Transformation Transformation;
        /** \brief point type */
        typedef typename Base::Point Point;
        /** \brief tangent vector type */
        typedef typename Base::Tangent Tangent;
        /** \brief adjoint transformation type */
        typedef typename Base::Adjoint Adjoint;

        // base is friend so unit_quaternion_nonconst can be accessed from base
        friend class SO3GroupBase<SO3Group<_Scalar, _Options> >;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        /**
         * \brief Default constructor
         *
         * Initialize Quaternion to identity rotation.
         */
        inline
        SO3Group()
                : unit_quaternion_(static_cast<Scalar> ( 1 ), static_cast<Scalar> ( 0 ),
                                   static_cast<Scalar> ( 0 ), static_cast<Scalar> ( 0 )) {
        }

        /**
         * \brief Copy constructor
         */
        template<typename OtherDerived>
        inline
        SO3Group(const SO3GroupBase<OtherDerived> &other)
                : unit_quaternion_(other.unit_quaternion()) {
        }

        /**
         * \brief Constructor from rotation matrix
         *
         * \pre rotation matrix need to be orthogonal with determinant of 1
         */
        inline SO3Group(const Transformation &R)
                : unit_quaternion_(R) {
        }

        /**
         * \brief Constructor from quaternion
         *
         * \pre quaternion must not be zero
         */
        inline explicit
        SO3Group(const Quaternion<Scalar> &quat) : unit_quaternion_(quat) {
            Base::normalize();
        }

        /**
         * \brief Constructor from Euler angles
         *
         * \param alpha1 rotation around x-axis
         * \param alpha2 rotation around y-axis
         * \param alpha3 rotation around z-axis
         *
         * Since rotations in 3D do not commute, the order of the individual rotations
         * matter. Here, the following convention is used. We calculate a SO3 member
         * corresponding to the rotation matrix \f$ R \f$ such
         * that \f$ R=\exp\left(\begin{array}{c}\alpha_1\\ 0\\ 0\end{array}\right)
         *    \cdot   \exp\left(\begin{array}{c}0\\ \alpha_2\\ 0\end{array}\right)
         *    \cdot   \exp\left(\begin{array}{c}0\\ 0\\ \alpha_3\end{array}\right)\f$.
         */
        inline
        SO3Group(Scalar alpha1, Scalar alpha2, Scalar alpha3) {
            const static Scalar zero = static_cast<Scalar> ( 0 );
            unit_quaternion_
                    = (SO3Group::exp(Tangent ( alpha1, zero, zero))
                              *SO3Group::exp(Tangent(zero, alpha2, zero))
                      * SO3Group::exp(Tangent(zero, zero, alpha3)))
            .unit_quaternion_;
        }

        /**
         * \brief Accessor of unit quaternion
         *
         * No direct write access is given to ensure the quaternion stays normalized.
         */
        EIGEN_STRONG_INLINE
        ConstQuaternionReference unit_quaternion() const {
            return unit_quaternion_;
        }

    protected:
        // Mutator of unit_quaternion is protected so users are hampered
        // from setting non-unit quaternions.
        EIGEN_STRONG_INLINE
        QuaternionReference unit_quaternion_nonconst() {
            return unit_quaternion_;
        }

        Quaternion<Scalar> unit_quaternion_;
    };

} // end namespace


namespace Eigen {
/**
 * \brief Specialisation of Eigen::Map for SO3GroupBase
 *
 * Allows us to wrap SO3 Objects around POD array
 * (e.g. external c style quaternion)
 */
    template<typename _Scalar, int _Options>
    class Map<Sophus::SO3Group<_Scalar>, _Options>
            : public Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options> > {
        typedef Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options> > Base;

    public:
        /** \brief scalar type */
        typedef typename internal::traits<Map>::Scalar Scalar;
        /** \brief quaternion reference type */
        typedef typename internal::traits<Map>::QuaternionType &
                QuaternionReference;
        /** \brief quaternion const reference type */
        typedef const typename internal::traits<Map>::QuaternionType &
                ConstQuaternionReference;

        /** \brief degree of freedom of group */
        static const int DoF = Base::DoF;
        /** \brief number of internal parameters used */
        static const int num_parameters = Base::num_parameters;
        /** \brief group transformations are NxN matrices */
        static const int N = Base::N;
        /** \brief group transfomation type */
        typedef typename Base::Transformation Transformation;
        /** \brief point type */
        typedef typename Base::Point Point;
        /** \brief tangent vector type */
        typedef typename Base::Tangent Tangent;
        /** \brief adjoint transformation type */
        typedef typename Base::Adjoint Adjoint;

        // base is friend so unit_quaternion_nonconst can be accessed from base
        friend class Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options> >;

        EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR (Map)

        using Base::operator*=;
        using Base::operator*;

        EIGEN_STRONG_INLINE
        Map(Scalar *coeffs) : unit_quaternion_(coeffs) {
        }

        /**
         * \brief Accessor of unit quaternion
         *
         * No direct write access is given to ensure the quaternion stays normalized.
         */
        EIGEN_STRONG_INLINE
        ConstQuaternionReference unit_quaternion() const {
            return unit_quaternion_;
        }

    protected:
        // Mutator of unit_quaternion is protected so users are hampered
        // from setting non-unit quaternions.
        EIGEN_STRONG_INLINE
        QuaternionReference unit_quaternion_nonconst() {
            return unit_quaternion_;
        }

        Map<Quaternion<Scalar>, _Options> unit_quaternion_;
    };

/**
 * \brief Specialisation of Eigen::Map for const SO3GroupBase
 *
 * Allows us to wrap SO3 Objects around POD array
 * (e.g. external c style quaternion)
 */
    template<typename _Scalar, int _Options>
    class Map<const Sophus::SO3Group<_Scalar>, _Options>
            : public Sophus::SO3GroupBase<
                    Map<const Sophus::SO3Group<_Scalar>, _Options> > {
        typedef Sophus::SO3GroupBase<Map<const Sophus::SO3Group<_Scalar>, _Options> >
                Base;

    public:
        /** \brief scalar type */
        typedef typename internal::traits<Map>::Scalar Scalar;
        /** \brief quaternion const reference type */
        typedef const typename internal::traits<Map>::QuaternionType &
                ConstQuaternionReference;

        /** \brief degree of freedom of group */
        static const int DoF = Base::DoF;
        /** \brief number of internal parameters used */
        static const int num_parameters = Base::num_parameters;
        /** \brief group transformations are NxN matrices */
        static const int N = Base::N;
        /** \brief group transfomation type */
        typedef typename Base::Transformation Transformation;
        /** \brief point type */
        typedef typename Base::Point Point;
        /** \brief tangent vector type */
        typedef typename Base::Tangent Tangent;
        /** \brief adjoint transformation type */
        typedef typename Base::Adjoint Adjoint;

        EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR (Map)

        using Base::operator*=;
        using Base::operator*;

        EIGEN_STRONG_INLINE
        Map(const Scalar *coeffs) : unit_quaternion_(coeffs) {
        }

        /**
         * \brief Accessor of unit quaternion
         *
         * No direct write access is given to ensure the quaternion stays normalized.
         */
        EIGEN_STRONG_INLINE
        const ConstQuaternionReference unit_quaternion() const {
            return unit_quaternion_;
        }

    protected:
        const Map<const Quaternion<Scalar>, _Options> unit_quaternion_;
    };

}

#endif