// This file is part of Sophus.
//
// Copyright 2012-2013 Hauke Strasdat
//
// 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_RXSO3_HPP
#define RXSO3_HPP

#include "sophus.hpp"
#include "so3.hpp"

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

namespace Sophus {
template<typename _Scalar, int _Options=0> class RxSO3Group;
typedef RxSO3Group<double> ScSO3 EIGEN_DEPRECATED;
typedef RxSO3Group<double> RxSO3d; /**< double precision RxSO3 */
typedef RxSO3Group<float> RxSO3f;  /**< single precision RxSO3 */
}

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

namespace Eigen {
namespace internal {

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

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

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

}
}

namespace Sophus {
using namespace Eigen;

class ScaleNotPositive : public SophusException {
public:
  ScaleNotPositive ()
    : SophusException("Scale factor is not positive") {
  }
};

/**
 * \brief RxSO3 base type - implements RxSO3 class but is storage agnostic
 *
 * This class implements the group \f$ R^{+} \times SO3 \f$ (RxSO3), the direct
 * product of the group of positive scalar matrices (=isomorph to the positive
 * real numbers) and the three-dimensional special orthogonal group SO3.
 * Geometrically, it is the group of rotation and scaling in three dimensions.
 * As a matrix groups, RxSO3 consists of matrices of the form \f$ s\cdot R \f$
 * where \f$ R \f$ is an orthognal matrix with \f$ det(R)=1 \f$ and \f$ s>0 \f$
 * be a positive real number.
 *
 * Internally, RxSO3 is represented by the group of non-zero quaternions. This
 * is a most compact representation since the degrees of freedom (DoF) of RxSO3
 * (=4) equals the number of internal parameters (=4).
 *
 * [add more detailed description/tutorial]
 */
template<typename Derived>
class RxSO3GroupBase {
public:
  /** \brief scalar type, use with care since this might be a Map 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 and one for scaling) */
  static const int DoF = 4;
  /** \brief number of internal parameters used
   *         (quaternion for rotation and scaling) */
  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 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.
   *
   * For RxSO3, it simply returns the rotation matrix corresponding to
   * \f$ A \f$.
   */
  inline
  const Adjoint Adj() const {
    Adjoint res;
    res.setIdentity();
    res.template topLeftCorner<3,3>() = rotationMatrix();
    return res;
  }

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

  /**
   * \returns pointer to internal data
   *
   * This provides direct read/write access to internal data. RxSO3 is
   * represented by an Eigen::Quaternion (four parameters).
   *
   * Note: The first three Scalars represent the imaginary parts, while the
   * forth Scalar represent the real part.
   */
  inline Scalar* data() {
    return quaternion().coeffs().data();
  }

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

  /**
   * \brief In-place group multiplication
   *
   *  Same as operator*=() for RxSO3.
   *
   * \see operator*=()
   */
  inline
  void fastMultiply(const RxSO3Group<Scalar>& other) {
    quaternion() *= other.quaternion();
  }


  /**
   * \returns group inverse of instance
   */
  inline
  const RxSO3Group<Scalar> inverse() const {
    if(quaternion().squaredNorm() <= static_cast<Scalar>(0)) {
      throw ScaleNotPositive();
    }
    return RxSO3Group<Scalar>(quaternion().inverse());
  }

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

  /**
   * \returns 3x3 matrix representation of instance
   *
   * For RxSO3, the matrix representation is a scaled orthogonal
   * matrix \f$ sR \f$ with \f$ det(sR)=s^3 \f$, thus a scaled rotation
   * matrix \f$ R \f$  with scale s.
   */
  inline
  const Transformation matrix() const {
    //ToDO: implement this directly!
    Scalar scale = quaternion().norm();
    Quaternion<Scalar> norm_quad = quaternion();
    norm_quad.coeffs() /= scale;
    return scale*norm_quad.toRotationMatrix();
  }

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

  /**
   * \brief Group multiplication
   * \see operator*=()
   */
  inline
  const RxSO3Group<Scalar> operator*(const RxSO3Group<Scalar>& other) const {
    RxSO3Group<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 scaled version of \f$p\f$
   *
   * This function rotates and scales a point \f$ p \f$ in  \f$ \mathbf{R}^3 \f$
   * by the RxSO3 transformation \f$sR\f$ (=rotation matrix)
   * : \f$ p' = sR\cdot p \f$.
   */
  inline
  const Point operator*(const Point & p) const {
    //ToDO: implement this directly!
    Scalar scale = quaternion().norm();
    Quaternion<Scalar> norm_quad = quaternion();
    norm_quad.coeffs() /= scale;
    return scale*norm_quad._transformVector(p);
  }

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

  /**
   * \brief Mutator of quaternion
   */
  EIGEN_STRONG_INLINE
  QuaternionReference quaternion() {
    return static_cast<Derived*>(this)->quaternion();
  }

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

  /**
   * \returns rotation matrix
   */
  inline
  Transformation rotationMatrix() const {
    Scalar scale = quaternion().norm();
    Quaternion<Scalar> norm_quad = quaternion();
    norm_quad.coeffs() /= scale;
    return norm_quad.toRotationMatrix();
  }

  /**
   * \returns scale
   */
  EIGEN_STRONG_INLINE
  const Scalar scale() const {
    return quaternion().norm();
  }

  /**
   * \brief Setter of quaternion using rotation matrix, leaves scale untouched
   *
   * \param R a 3x3 rotation matrix
   * \pre       the 3x3 matrix should be orthogonal and have a determinant of 1
   */
  inline
  void setRotationMatrix(const Transformation & R) {
    Scalar saved_scale = scale();
    quaternion() = R;
    quaternion().coeffs() *= saved_scale;
  }

  /**
   * \brief Scale setter
   */
  EIGEN_STRONG_INLINE
  void setScale(const Scalar & scale) {
    quaternion().normalize();
    quaternion().coeffs() *= scale;
  }

  /**
   * \brief Setter of quaternion using scaled rotation matrix
   *
   * \param sR a 3x3 scaled rotation matrix
   * \pre        the 3x3 matrix should be "scaled orthogonal"
   *             and have a positive determinant
   */
  inline
  void setScaledRotationMatrix
  (const Transformation & sR) {
    Transformation squared_sR = sR*sR.transpose();
    Scalar squared_scale
        = static_cast<Scalar>(1./3.)
        *(squared_sR(0,0)+squared_sR(1,1)+squared_sR(2,2));
    if (squared_scale <= static_cast<Scalar>(0)) {
      throw ScaleNotPositive();
    }
    Scalar scale = std::sqrt(squared_scale);
    if (scale <= static_cast<Scalar>(0)) {
      throw ScaleNotPositive();
    }
    quaternion() = sR/scale;
    quaternion().coeffs() *= scale;
  }

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

  /**
   * \param   b 4-vector representation of Lie algebra element
   * \returns   derivative of Lie bracket
   *
   * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{rxso3} \f$
   * with \f$ [a, b]_{rxso3} \f$ being the lieBracket() of the Lie
   * algebra rxso3.
   *
   * \see lieBracket()
   */
  inline static
  const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
    Adjoint res;
    res.setZero();
    res.template topLeftCorner<3,3>() = -SO3::hat(b.template head<3>());
    return res;
  }

  /**
   * \brief Group exponential
   *
   * \param a tangent space element
   *          (rotation vector \f$ \omega \f$ and logarithm of scale)
   * \returns corresponding element of the group RxSO3
   *
   * 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 RxSO3.
   *
   * \see expAndTheta()
   * \see hat()
   * \see log()
   */
  inline static
  const RxSO3Group<Scalar> exp(const Tangent & a) {
    Scalar theta;
    return expAndTheta(a, &theta);
  }

  /**
   * \brief Group exponential and theta
   *
   * \param      a     tangent space element
   *                   (rotation vector \f$ \omega \f$ and logarithm of scale )
   * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
   * \returns          corresponding element of the group RxSO3
   *
   * \see exp() for details
   */
  inline static
  const RxSO3Group<Scalar> expAndTheta(const Tangent & a,
                                       Scalar * theta) {
    const Matrix<Scalar,3,1> & omega = a.template head<3>();
    Scalar sigma = a[3];
    Scalar scale = std::exp(sigma);
    Quaternion<Scalar> quat
        = SO3Group<Scalar>::expAndTheta(omega, theta).unit_quaternion();
    quat.coeffs() *= scale;
    return RxSO3Group<Scalar>(quat);
  }

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

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

  /**
   * \brief Lie bracket
   *
   * \param a 4-vector representation of Lie algebra element
   * \param b 4-vector representation of Lie algebra element
   * \returns 4-vector representation of Lie algebra element
   *
   * It computes the bracket of RxSO3. To be more specific, it
   * computes \f$ [a, 2]_{rxso3}
   * := [\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 RxSO3.
   *
   * \see hat()
   * \see vee()
   */
  inline static
  const Tangent lieBracket(const Tangent & a,
                           const Tangent & b) {
    const Matrix<Scalar,3,1> & omega1 = a.template head<3>();
    const Matrix<Scalar,3,1> & omega2 = b.template head<3>();
    Matrix<Scalar,4,1> res;
    res.template head<3>() = omega1.cross(omega2);
    res[3] = static_cast<Scalar>(0);
    return res;
  }

  /**
   * \brief Logarithmic map
   *
   * \param other element of the group RxSO3
   * \returns     corresponding tangent space element
   *              (rotation vector \f$ \omega \f$ and logarithm of scale)
   *
   * 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 RxSO3.
   *
   * \see exp()
   * \see logAndTheta()
   * \see vee()
   */
  inline static
  const Tangent log(const RxSO3Group<Scalar> & other) {
    Scalar theta;
    return logAndTheta(other, &theta);
  }

  /**
   * \brief Logarithmic map and theta
   *
   * \param      other element of the group RxSO3
   * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
   * \returns          corresponding tangent space element
   *                   (rotation vector \f$ \omega \f$ and logarithm of scale)
   *
   * \see log() for details
   */
  inline static
  const Tangent logAndTheta(const RxSO3Group<Scalar> & other,
                            Scalar * theta) {
    const Scalar & scale = other.quaternion().norm();
    Tangent omega_sigma;
    omega_sigma[3] = std::log(scale);
    omega_sigma.template head<3>()
        = SO3Group<Scalar>::logAndTheta(SO3Group<Scalar>(other.quaternion()),
                                        theta);
    return omega_sigma;
  }

  /**
   * \brief vee-operator
   *
   * \param Omega 3x3-matrix representation of Lie algebra element
   * \returns     4-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 Tangent( static_cast<Scalar>(0.5) * (Omega(2,1) - Omega(1,2)),
                    static_cast<Scalar>(0.5) * (Omega(0,2) - Omega(2,0)),
                    static_cast<Scalar>(0.5) * (Omega(1,0) - Omega(0,1)),
                    static_cast<Scalar>(1./3.)
                    * (Omega(0,0) + Omega(1,1) + Omega(2,2)) );
  }
};

/**
 * \brief RxSO3 default type - Constructors and default storage for RxSO3 Type
 */
template<typename _Scalar, int _Options>
class RxSO3Group : public RxSO3GroupBase<RxSO3Group<_Scalar,_Options> > {
  typedef RxSO3GroupBase<RxSO3Group<_Scalar,_Options> > Base;
public:
  /** \brief scalar type */
  typedef typename internal::traits<SO3Group<_Scalar,_Options> >
  ::Scalar Scalar;
  /** \brief quaternion reference type */
  typedef typename internal::traits<SO3Group<_Scalar,_Options> >
  ::QuaternionType & QuaternionReference;
  /** \brief quaternion const reference type */
  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;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

  /**
   * \brief Copy constructor
   */
  template<typename OtherDerived> inline
  RxSO3Group(const RxSO3GroupBase<OtherDerived> & other)
    : quaternion_(other.quaternion()) {
  }

  /**
   * \brief Constructor from scaled rotation matrix
   *
   * \pre matrix need to be "scaled orthogonal" with positive determinant
   */
  inline explicit
  RxSO3Group(const Transformation & sR) {
    this->setScaledRotationMatrix(sR);
  }

  /**
   * \brief Constructor from scale factor and rotation matrix
   *
   * \pre rotation matrix need to be orthogonal with determinant of 1
   * \pre scale need to be not zero
   */
  inline
  RxSO3Group(const Scalar & scale, const Transformation & R)
    : quaternion_(R) {
    if(scale <= static_cast<Scalar>(0)) {
      throw ScaleNotPositive();
    }
    quaternion_.normalize();
    quaternion_.coeffs() *= scale;
  }

  /**
   * \brief Constructor from scale factor and SO3
   *
   * \pre scale need to be not zero
   */
  inline
  RxSO3Group(const Scalar & scale, const SO3Group<Scalar> & so3)
    : quaternion_(so3.unit_quaternion()) {
    if (scale <= static_cast<Scalar>(0)) {
      throw ScaleNotPositive();
    }
    quaternion_.normalize();
    quaternion_.coeffs() *= scale;
  }

  /**
   * \brief Constructor from quaternion
   *
   * \pre quaternion must not be zero
   */
  inline explicit
  RxSO3Group(const Quaternion<Scalar> & quat) : quaternion_(quat) {
    if(quaternion_.squaredNorm() <= SophusConstants<Scalar>::epsilon()) {
      throw ScaleNotPositive();
    }
  }

  /**
   * \brief Mutator of quaternion
   */
  EIGEN_STRONG_INLINE
  QuaternionReference quaternion() {
    return quaternion_;
  }

  /**
   * \brief Accessor of quaternion
   */
  EIGEN_STRONG_INLINE
  ConstQuaternionReference quaternion() const {
    return quaternion_;
  }

protected:
  Quaternion<Scalar> quaternion_;
};

} // end namespace


namespace Eigen {
/**
 * \brief Specialisation of Eigen::Map for RxSO3GroupBase
 *
 * Allows us to wrap RxSO3 Objects around POD array
 * (e.g. external c style quaternion)
 */
template<typename _Scalar, int _Options>
class Map<Sophus::RxSO3Group<_Scalar>, _Options>
    : public Sophus::RxSO3GroupBase<
    Map<Sophus::RxSO3Group<_Scalar>,_Options> > {
  typedef Sophus::RxSO3GroupBase<Map<Sophus::RxSO3Group<_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;

  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
  using Base::operator*=;
  using Base::operator*;

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

  /**
   * \brief Mutator of quaternion
   */
  EIGEN_STRONG_INLINE
  QuaternionReference quaternion() {
    return quaternion_;
  }

  /**
   * \brief Accessor of quaternion
   */
  EIGEN_STRONG_INLINE
  ConstQuaternionReference quaternion() const {
    return quaternion_;
  }

protected:
  Map<Quaternion<Scalar>,_Options> quaternion_;
};

/**
 * \brief Specialisation of Eigen::Map for const RxSO3GroupBase
 *
 * Allows us to wrap RxSO3 Objects around POD array
 * (e.g. external c style quaternion)
 */
template<typename _Scalar, int _Options>
class Map<const Sophus::RxSO3Group<_Scalar>, _Options>
    : public Sophus::RxSO3GroupBase<
    Map<const Sophus::RxSO3Group<_Scalar>, _Options> > {
  typedef Sophus::RxSO3GroupBase<
  Map<const Sophus::RxSO3Group<_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) : quaternion_(coeffs) {
  }

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

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

}

#endif // SOPHUS_RXSO3_HPP