// 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 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