GAAS

// This file is part of Sophus.
//
// Copyright 2011-2013 Hauke Strasdat
//           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_SE3_HPP
#define SOPHUS_SE3_HPP

#include "so3.hpp"

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

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

    typedef SE3Group<double> SE3 EIGEN_DEPRECATED;
    typedef SE3Group<double> SE3d; /**< double precision SE3 */
    typedef SE3Group<float> SE3f;  /**< single precision SE3 */
    typedef Matrix<double, 6, 1> Vector6d;
    typedef Matrix<double, 6, 6> Matrix6d;
    typedef Matrix<float, 6, 1> Vector6f;
    typedef Matrix<float, 6, 6> Matrix6f;
}

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

namespace Eigen {
    namespace internal {

        template<typename _Scalar, int _Options>
        struct traits<Sophus::SE3Group<_Scalar, _Options> > {
            typedef _Scalar Scalar;
            typedef Matrix<Scalar, 3, 1> TranslationType;
            typedef Sophus::SO3Group<Scalar> SO3Type;
        };

        template<typename _Scalar, int _Options>
        struct traits<Map < Sophus::SE3Group<_Scalar>, _Options> >
        : traits<Sophus::SE3Group<_Scalar, _Options> > {
        typedef _Scalar Scalar;
        typedef Map <Matrix<Scalar, 3, 1>, _Options> TranslationType;
        typedef Map <Sophus::SO3Group<Scalar>, _Options> SO3Type;
    };

    template<typename _Scalar, int _Options>
    struct traits<Map<const Sophus::SE3Group<_Scalar>, _Options> >
            : traits<const Sophus::SE3Group<_Scalar, _Options> > {
        typedef _Scalar Scalar;
        typedef Map<const Matrix<Scalar, 3, 1>, _Options> TranslationType;
        typedef Map<const Sophus::SO3Group<Scalar>, _Options> SO3Type;
    };

}
}

namespace Sophus {
    using namespace Eigen;
    using namespace std;

/**
 * \brief SE3 base type - implements SE3 class but is storage agnostic
 *
 * [add more detailed description/tutorial]
 */
    template<typename Derived>
    class SE3GroupBase {
    public:
        /** \brief scalar type */
        typedef typename internal::traits<Derived>::Scalar Scalar;
        /** \brief translation reference type */
        typedef typename internal::traits<Derived>::TranslationType &
                TranslationReference;
        /** \brief translation const reference type */
        typedef const typename internal::traits<Derived>::TranslationType &
                ConstTranslationReference;
        /** \brief SO3 reference type */
        typedef typename internal::traits<Derived>::SO3Type &
                SO3Reference;
        /** \brief SO3 const reference type */
        typedef const typename internal::traits<Derived>::SO3Type &
                ConstSO3Reference;

        /** \brief degree of freedom of group
          *        (three for translation, three for rotation) */
        static const int DoF = 6;
        /** \brief number of internal parameters used
         *         (unit quaternion for rotation + translation 3-vector) */
        static const int num_parameters = 7;
        /** \brief group transformations are NxN matrices */
        static const int N = 4;
        /** \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 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()-operator.
         */
        inline
        const Adjoint Adj() const {
            const Matrix<Scalar, 3, 3> &R = so3().matrix();
            Adjoint res;
            res.block(0, 0, 3, 3) = R;
            res.block(3, 3, 3, 3) = R;
            res.block(0, 3, 3, 3) = SO3Group<Scalar>::hat(translation()) * R;
            res.block(3, 0, 3, 3) = Matrix<Scalar, 3, 3>::Zero(3, 3);
            return res;
        }

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

        /**
         * \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 SE3Group<Scalar> &other) {
            translation() += so3() * (other.translation());
            so3().fastMultiply(other.so3());
        }

        /**
         * \returns Group inverse of instance
         */
        inline
        const SE3Group<Scalar> inverse() const {
            const SO3Group<Scalar> invR = so3().inverse();
            return SE3Group<Scalar>(invR, invR * (translation()
                                                  * static_cast<Scalar> ( -1 )));
        }

        /**
         * \brief Logarithmic map
         *
         * \returns tangent space representation
         *          (translational part and rotation vector) of instance
         *
         * \see  log().
         */
        inline
        const Tangent log() const {
            return log(*this);
        }

        /**
         * \brief Normalize SO3 element
         *
         * It re-normalizes the SO3 element. This method only needs to
         * be called in conjunction with fastMultiply() or data() write access.
         */
        inline
        void normalize() {
            so3().normalize();
        }

        /**
         * \returns 4x4 matrix representation of instance
         */
        inline
        const Transformation matrix() const {
            Transformation homogenious_matrix;
            homogenious_matrix.setIdentity();
            homogenious_matrix.block(0, 0, 3, 3) = rotationMatrix();
            homogenious_matrix.col(3).head(3) = translation();
            return homogenious_matrix;
        }

        /**
         * \returns 3x4 matrix representation of instance
         *
         * It returns the three first row of matrix().
         */
        inline
        const Matrix<Scalar, 3, 4> matrix3x4() const {
            Matrix<Scalar, 3, 4> matrix;
            matrix.block(0, 0, 3, 3) = rotationMatrix();
            matrix.col(3) = translation();
            return matrix;
        }

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

        /**
         * \brief Group multiplication
         * \see operator*=()
         */
        inline
        const SE3Group<Scalar> operator*(const SE3Group<Scalar> &other) const {
            SE3Group<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 and translated version of \f$p\f$
         *
         * This function rotates and translates point \f$ p \f$
         * in \f$ \mathbf{R}^3 \f$ by the SE3 transformation \f$R,t\f$
         * (=rotation matrix, translation vector): \f$ p' = R\cdot p + t \f$.
         */
        inline
        const Point operator*(const Point &p) const {
            return so3() * p + translation();
        }

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


        /**
         * \returns Rotation matrix
         *
         * deprecated: use rotationMatrix() instead.
         */
        typedef Transformation M3_marcos_dont_like_commas;

        inline
        EIGEN_DEPRECATED const M3_marcos_dont_like_commas rotation_matrix() const {
            return so3().matrix();
        }

        /**
         * \returns Rotation matrix
         */
        inline
        const Matrix<Scalar, 3, 3> rotationMatrix() const {
            return so3().matrix();
        }


        /**
         * \brief Mutator of SO3 group
         */
        EIGEN_STRONG_INLINE
        SO3Reference so3() {
            return static_cast<Derived *> ( this )->so3();
        }

        /**
         * \brief Accessor of SO3 group
         */
        EIGEN_STRONG_INLINE
        ConstSO3Reference so3() const {
            return static_cast<const Derived *> ( this )->so3();
        }

        /**
         * \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> &quat) {
            return so3().setQuaternion(quat);
        }

        /**
         * \brief Setter of unit quaternion using rotation matrix
         *
         * \param rotation_matrix a 3x3 rotation matrix
         * \pre   the 3x3 matrix should be orthogonal and have a determinant of 1
         */
        inline
        void setRotationMatrix
                (const Matrix<Scalar, 3, 3> &rotation_matrix) {
            so3().setQuaternion(Quaternion<Scalar>(rotation_matrix));
        }

        /**
         * \brief Mutator of translation vector
         */
        EIGEN_STRONG_INLINE
        TranslationReference translation() {
            return static_cast<Derived *> ( this )->translation();
        }

        /**
         * \brief Accessor of translation vector
         */
        EIGEN_STRONG_INLINE
        ConstTranslationReference translation() const {
            return static_cast<const Derived *> ( this )->translation();
        }

        /**
         * \brief Accessor of unit quaternion
         *
         * No direct write access is given to ensure the quaternion stays normalized.
         */
        inline
        typename internal::traits<Derived>::SO3Type::ConstQuaternionReference
        unit_quaternion() const {
            return so3().unit_quaternion();
        }

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

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

            const Matrix<Scalar, 3, 1> &upsilon2 = b.template head<3>();
            const Matrix<Scalar, 3, 1> &omega2 = b.template tail<3>();

            res.template topLeftCorner<3, 3>() = -SO3Group<Scalar>::hat(omega2);
            res.template topRightCorner<3, 3>() = -SO3Group<Scalar>::hat(upsilon2);
            res.template bottomRightCorner<3, 3>() = -SO3Group<Scalar>::hat(omega2);
            return res;
        }

        /**
         * \brief Group exponential
         *
         * \param a tangent space element (6-vector)
         * \returns corresponding element of the group SE3
         *
         * The first three components of \f$ a \f$ represent the translational
         * part \f$ \upsilon \f$ in the tangent space of SE3, while the last three
         * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$.
         *
         * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
         * with \f$ \exp(\cdot) \f$ being the matrix exponential
         * and \f$ \widehat{\cdot} \f$ the hat()-operator of SE3.
         *
         * \see hat()
         * \see log()
         */
        inline static
        const SE3Group<Scalar> exp(const Tangent &a) {
            const Matrix<Scalar, 3, 1> &omega = a.template tail<3>();

            Scalar theta;
            const SO3Group<Scalar> &so3
                    = SO3Group<Scalar>::expAndTheta(omega, &theta);

            const Matrix<Scalar, 3, 3> &Omega = SO3Group<Scalar>::hat(omega);
            const Matrix<Scalar, 3, 3> &Omega_sq = Omega * Omega;
            Matrix<Scalar, 3, 3> V;

            if (theta < SophusConstants<Scalar>::epsilon()) {
                V = so3.matrix();
                //Note: That is an accurate expansion!
            } else {
                Scalar theta_sq = theta * theta;
                V = (Matrix<Scalar, 3, 3>::Identity()
                     + (static_cast<Scalar> ( 1 ) - std::cos(theta)) / (theta_sq) * Omega
                     + (theta - std::sin(theta)) / (theta_sq * theta) * Omega_sq);
            }
            return SE3Group<Scalar>(so3, V * a.template head<3>());
        }

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

        /**
         * \brief hat-operator
         *
         * \param omega 6-vector representation of Lie algebra element
         * \returns     4x4-matrix representatin of Lie algebra element
         *
         * Formally, the hat-operator of SE3 is defined
         * as \f$ \widehat{\cdot}: \mathbf{R}^6 \rightarrow \mathbf{R}^{4\times 4},
         * \quad \widehat{\omega} = \sum_{i=0}^5 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 &v) {
            Transformation Omega;
            Omega.setZero();
            Omega.template topLeftCorner<3, 3>()
                    = SO3Group<Scalar>::hat(v.template tail<3>());
            Omega.col(3).template head<3>() = v.template head<3>();
            return Omega;
        }

        /**
         * \brief Lie bracket
         *
         * \param a 6-vector representation of Lie algebra element
         * \param b 6-vector representation of Lie algebra element
         * \returns      6-vector representation of Lie algebra element
         *
         * It computes the bracket of SE3. To be more specific, it
         * computes \f$ [a, b]_{se3}
         * := [\widehat{a}, \widehat{b}]^\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 SE3.
         *
         * \see hat()
         * \see vee()
         */
        inline static
        const Tangent lieBracket(const Tangent &a,
                                 const Tangent &b) {
            Matrix<Scalar, 3, 1> upsilon1 = a.template head<3>();
            Matrix<Scalar, 3, 1> upsilon2 = b.template head<3>();
            Matrix<Scalar, 3, 1> omega1 = a.template tail<3>();
            Matrix<Scalar, 3, 1> omega2 = b.template tail<3>();

            Tangent res;
            res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2);
            res.template tail<3>() = omega1.cross(omega2);

            return res;
        }

        /**
         * \brief Logarithmic map
         *
         * \param other element of the group SE3
         * \returns     corresponding tangent space element
         *              (translational part \f$ \upsilon \f$
         *               and 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 SE3.
         *
         * \see exp()
         * \see vee()
         */
        inline static
        const Tangent log(const SE3Group<Scalar> &se3) {
            Tangent upsilon_omega;
            Scalar theta;
            upsilon_omega.template tail<3>()
                    = SO3Group<Scalar>::logAndTheta(se3.so3(), &theta);

            if (std::abs(theta) < SophusConstants<Scalar>::epsilon()) {
                const Matrix<Scalar, 3, 3> &Omega
                        = SO3Group<Scalar>::hat(upsilon_omega.template tail<3>());
                const Matrix<Scalar, 3, 3> &V_inv =
                        Matrix<Scalar, 3, 3>::Identity() -
                        static_cast<Scalar> ( 0.5 ) * Omega
                        + static_cast<Scalar> ( 1. / 12. ) * (Omega * Omega);

                upsilon_omega.template head<3>() = V_inv * se3.translation();
            } else {
                const Matrix<Scalar, 3, 3> &Omega
                        = SO3Group<Scalar>::hat(upsilon_omega.template tail<3>());
                const Matrix<Scalar, 3, 3> &V_inv =
                        (Matrix<Scalar, 3, 3>::Identity() - static_cast<Scalar> ( 0.5 ) * Omega
                         + (static_cast<Scalar> ( 1 )
                            - theta / (static_cast<Scalar> ( 2 ) * tan(theta / Scalar(2)))) /
                           (theta * theta) * (Omega * Omega));
                upsilon_omega.template head<3>() = V_inv * se3.translation();
            }
            return upsilon_omega;
        }

        /**
         * \brief vee-operator
         *
         * \param Omega 4x4-matrix representation of Lie algebra element
         * \returns     6-vector representatin of Lie algebra element
         *
         * This is the inverse of the hat()-operator.
         *
         * \see hat()
         */
        inline static
        const Tangent vee(const Transformation &Omega) {
            Tangent upsilon_omega;
            upsilon_omega.template head<3>() = Omega.col(3).template head<3>();
            upsilon_omega.template tail<3>()
                    = SO3Group<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
            return upsilon_omega;
        }
    };

/**
 * \brief SE3 default type - Constructors and default storage for SE3 Type
 */
    template<typename _Scalar, int _Options>
    class SE3Group : public SE3GroupBase<SE3Group<_Scalar, _Options> > {
        typedef SE3GroupBase<SE3Group<_Scalar, _Options> > Base;
    public:
        /** \brief scalar type */
        typedef typename internal::traits<SE3Group<_Scalar, _Options> >
        ::Scalar Scalar;
        /** \brief SO3 reference type */
        typedef typename internal::traits<SE3Group<_Scalar, _Options> >
        ::SO3Type &SO3Reference;
        /** \brief SO3 const reference type */
        typedef const typename internal::traits<SE3Group<_Scalar, _Options> >
        ::SO3Type &ConstSO3Reference;
        /** \brief translation reference type */
        typedef typename internal::traits<SE3Group<_Scalar, _Options> >
        ::TranslationType &TranslationReference;
        /** \brief translation const reference type */
        typedef const typename internal::traits<SE3Group<_Scalar, _Options> >
        ::TranslationType &ConstTranslationReference;

        /** \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_MAKE_ALIGNED_OPERATOR_NEW

        /**
         * \brief Default constructor
         *
         * Initialize Quaternion to identity rotation and translation to zero.
         */
        inline
        SE3Group()
                : translation_(Matrix<Scalar, 3, 1>::Zero()) {
        }

        /**
         * \brief Copy constructor
         */
        template<typename OtherDerived>
        inline
        SE3Group(const SE3GroupBase<OtherDerived> &other)
                : so3_(other.so3()), translation_(other.translation()) {
        }

        /**
         * \brief Constructor from SO3 and translation vector
         */
        template<typename OtherDerived>
        inline
        SE3Group(const SO3GroupBase <OtherDerived> &so3,
                 const Point &translation)
                : so3_(so3), translation_(translation) {
        }

        /**
         * \brief Constructor from rotation matrix and translation vector
         *
         * \pre rotation matrix need to be orthogonal with determinant of 1
         */
        inline
        SE3Group(const Matrix<Scalar, 3, 3> &rotation_matrix,
                 const Point &translation)
                : so3_(rotation_matrix), translation_(translation) {
        }

        /**
         * \brief Constructor from quaternion and translation vector
         *
         * \pre quaternion must not be zero
         */
        inline
        SE3Group(const Quaternion <Scalar> &quaternion,
                 const Point &translation)
                : so3_(quaternion), translation_(translation) {
        }

        /**
         * \brief Constructor from 4x4 matrix
         *
         * \pre top-left 3x3 sub-matrix need to be orthogonal with determinant of 1
         */
        inline explicit
        SE3Group(const Eigen::Matrix<Scalar, 4, 4> &T)
                : so3_(T.template topLeftCorner<3, 3>()),
                  translation_(T.template block<3, 1>(0, 3)) {
        }

        /**
         * \returns pointer to internal data
         *
         * This provides unsafe read/write access to internal data. SE3 is represented
         * by a pair of an SO3 element (4 parameters) and translation vector (three
         * parameters). 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()
         */
        EIGEN_STRONG_INLINE
        Scalar *data() {
            // so3_ and translation_ are layed out sequentially with no padding
            return so3_.data();
        }

        /**
         * \returns const pointer to internal data
         *
         * Const version of data().
         */
        EIGEN_STRONG_INLINE
        const Scalar *data() const {
            // so3_ and translation_ are layed out sequentially with no padding
            return so3_.data();
        }

        /**
         * \brief Accessor of SO3
         */
        EIGEN_STRONG_INLINE
        SO3Reference so3() {
            return so3_;
        }

        /**
         * \brief Mutator of SO3
         */
        EIGEN_STRONG_INLINE
        ConstSO3Reference so3() const {
            return so3_;
        }

        /**
         * \brief Mutator of translation vector
         */
        EIGEN_STRONG_INLINE
        TranslationReference translation() {
            return translation_;
        }

        /**
         * \brief Accessor of translation vector
         */
        EIGEN_STRONG_INLINE
        ConstTranslationReference translation() const {
            return translation_;
        }

    protected:
        Sophus::SO3Group<Scalar> so3_;
        Matrix<Scalar, 3, 1> translation_;
    };


} // end namespace


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

    public:
        /** \brief scalar type */
        typedef typename internal::traits<Map>::Scalar Scalar;
        /** \brief translation reference type */
        typedef typename internal::traits<Map>::TranslationType &
                TranslationReference;
        /** \brief translation const reference type */
        typedef const typename internal::traits<Map>::TranslationType &
                ConstTranslationReference;
        /** \brief SO3 reference type */
        typedef typename internal::traits<Map>::SO3Type &
                SO3Reference;
        /** \brief SO3 const reference type */
        typedef const typename internal::traits<Map>::SO3Type &
                ConstSO3Reference;

        /** \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(Scalar *coeffs)
                : so3_(coeffs),
                  translation_(coeffs + Sophus::SO3Group<Scalar>::num_parameters) {
        }

        /**
         * \brief Mutator of SO3
         */
        EIGEN_STRONG_INLINE
        SO3Reference so3() {
            return so3_;
        }

        /**
         * \brief Accessor of SO3
         */
        EIGEN_STRONG_INLINE
        ConstSO3Reference so3() const {
            return so3_;
        }

        /**
         * \brief Mutator of translation vector
         */
        EIGEN_STRONG_INLINE
        TranslationReference translation() {
            return translation_;
        }

        /**
         * \brief Accessor of translation vector
         */
        EIGEN_STRONG_INLINE
        ConstTranslationReference translation() const {
            return translation_;
        }

    protected:
        Map<Sophus::SO3Group<Scalar>, _Options> so3_;
        Map<Matrix < Scalar, 3, 1>,_Options>
        translation_;
    };

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

    public:
        /** \brief scalar type */
        typedef typename internal::traits<Map>::Scalar Scalar;
        /** \brief translation const reference type */
        typedef const typename internal::traits<Map>::TranslationType &
                ConstTranslationReference;
        /** \brief SO3 const reference type */
        typedef const typename internal::traits<Map>::SO3Type &
                ConstSO3Reference;

        /** \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)
                : so3_(coeffs),
                  translation_(coeffs + Sophus::SO3Group<Scalar>::num_parameters) {
        }

        EIGEN_STRONG_INLINE
        Map(const Scalar *trans_coeffs, const Scalar *rot_coeffs)
                : translation_(trans_coeffs), so3_(rot_coeffs) {
        }

        /**
         * \brief Accessor of SO3
         */
        EIGEN_STRONG_INLINE
        ConstSO3Reference so3() const {
            return so3_;
        }

        /**
         * \brief Accessor of translation vector
         */
        EIGEN_STRONG_INLINE
        ConstTranslationReference translation() const {
            return translation_;
        }

    protected:
        const Map<const Sophus::SO3Group<Scalar>, _Options> so3_;
        const Map<const Matrix<Scalar, 3, 1>, _Options> translation_;
    };

}

#endif