// 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_SE2_HPP
#define SOPHUS_SE2_HPP

#include "so2.hpp"

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

namespace Sophus {
template<typename _Scalar, int _Options=0> class SE2Group;
typedef SE2Group<double> SE2 EIGEN_DEPRECATED;
typedef SE2Group<double> SE2d; /**< double precision SE2 */
typedef SE2Group<float> SE2f;  /**< single precision SE2 */
}

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

namespace Eigen {
namespace internal {

template<typename _Scalar, int _Options>
struct traits<Sophus::SE2Group<_Scalar,_Options> > {
  typedef _Scalar Scalar;
  typedef Matrix<Scalar,2,1> TranslationType;
  typedef Sophus::SO2Group<Scalar> SO2Type;
};

template<typename _Scalar, int _Options>
struct traits<Map<Sophus::SE2Group<_Scalar>, _Options> >
    : traits<Sophus::SE2Group<_Scalar, _Options> > {
  typedef _Scalar Scalar;
  typedef Map<Matrix<Scalar,2,1>,_Options> TranslationType;
  typedef Map<Sophus::SO2Group<Scalar>,_Options> SO2Type;
};

template<typename _Scalar, int _Options>
struct traits<Map<const Sophus::SE2Group<_Scalar>, _Options> >
    : traits<const Sophus::SE2Group<_Scalar, _Options> > {
  typedef _Scalar Scalar;
  typedef Map<const Matrix<Scalar,2,1>,_Options> TranslationType;
  typedef Map<const Sophus::SO2Group<Scalar>,_Options> SO2Type;
};

}
}

namespace Sophus {
using namespace Eigen;
using namespace std;

/**
 * \brief SE2 base type - implements SE2 class but is storage agnostic
 *
 * [add more detailed description/tutorial]
 */
template<typename Derived>
class SE2GroupBase {
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 SO2 reference type */
  typedef typename internal::traits<Derived>::SO2Type &
  SO2Reference;
  /** \brief SO2 type */
  typedef const typename internal::traits<Derived>::SO2Type &
  ConstSO2Reference;

  /** \brief degree of freedom of group
    *        (two for translation, one for in-plane rotation) */
  static const int DoF = 3;
  /** \brief number of internal parameters used
    *        (unit complex number for rotation + translation 2-vector) */
  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,2,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,2,2> & R = so2().matrix();
    Transformation res;
    res.setIdentity();
    res.template topLeftCorner<2,2>() = R;
    res(0,2) =  translation()[1];
    res(1,2) = -translation()[0];
    return res;
  }

  /**
   * \returns copy of instance casted to NewScalarType
   */
  template<typename NewScalarType>
  inline SE2Group<NewScalarType> cast() const {
    return
        SE2Group<NewScalarType>(so2().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 SE2Group<Scalar>& other) {
    translation() += so2()*(other.translation());
    so2().fastMultiply(other.so2());
  }

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

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

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

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

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

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

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

  /**
   * \brief Group action on \f$ \mathbf{R}^2 \f$
   *
   * \param p point \f$p \in \mathbf{R}^2 \f$
   * \returns point \f$p' \in \mathbf{R}^2 \f$,
   *          rotated and translated version of \f$p\f$
   *
   * This function rotates and translates point \f$ p \f$
   * in \f$ \mathbf{R}^2 \f$ by the SE2 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 so2()*p + translation();
  }

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


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

  /**
   * \brief Setter of internal unit complex number representation
   *
   * \param complex
   * \pre   the complex number must not be zero
   *
   * The complex number is normalized to unit length.
   */
  inline
  void setComplex(const Matrix<Scalar,2,1> & complex) {
    return so2().setComplex(complex);
  }

  /**
   * \brief Setter of unit complex number using rotation matrix
   *
   * \param R a 2x2 matrix
   * \pre     the 2x2 matrix should be orthogonal and have a determinant of 1
   */
  inline
  void setRotationMatrix(const Matrix<Scalar,2,2> & R) {
    so2().setComplex(static_cast<Scalar>(0.5)*(R(0,0)+R(1,1)),
                     static_cast<Scalar>(0.5)*(R(1,0)-R(0,1)));
  }

  /**
   * \brief Mutator of SO2 group
   */
  EIGEN_STRONG_INLINE
  SO2Reference so2() {
    return static_cast<Derived*>(this)->so2();
  }

  /**
   * \brief Accessor of SO2 group
   */
  EIGEN_STRONG_INLINE
  ConstSO2Reference so2() const {
    return static_cast<const Derived*>(this)->so2();
  }

  /**
   * \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 complex number
   *
   * No direct write access is given to ensure the complex number stays
   * normalized.
   */
  inline
  typename internal::traits<Derived>::SO2Type::ConstComplexReference
  unit_complex() const {
    return so2().unit_complex();
  }

  ////////////////////////////////////////////////////////////////////////////
  // 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]_{se2} \f$
   * with \f$ [a, b]_{se2} \f$ being the lieBracket() of the Lie algebra se2.
   *
   * \see lieBracket()
   */
  inline static
  const Transformation d_lieBracketab_by_d_a(const Tangent & b) {
    static const Scalar zero = static_cast<Scalar>(0);
    Matrix<Scalar,2,1> upsilon2 = b.template head<2>();
    Scalar theta2 = b[2];

    Transformation res;
    res <<    zero, theta2, -upsilon2[1]
        ,  -theta2,   zero,  upsilon2[0]
        ,     zero,   zero,         zero;
    return res;
  }

  /**
   * \brief Group exponential
   *
   * \param a tangent space element (3-vector)
   * \returns corresponding element of the group SE2
   *
   * The first two components of \f$ a \f$ represent the translational
   * part \f$ \upsilon \f$ in the tangent space of SE2, while the last
   * components of \f$ a \f$ is the rotation angle \f$ \theta \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 SE2.
   *
   * \see hat()
   * \see log()
   */
  inline static
  const SE2Group<Scalar> exp(const Tangent & a) {
    Scalar theta = a[2];
    const SO2Group<Scalar> & so2 = SO2Group<Scalar>::exp(theta);
    Scalar sin_theta_by_theta;
    Scalar one_minus_cos_theta_by_theta;

    if(std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
      Scalar theta_sq = theta*theta;
      sin_theta_by_theta
          = static_cast<Scalar>(1.) - static_cast<Scalar>(1./6.)*theta_sq;
      one_minus_cos_theta_by_theta
          = static_cast<Scalar>(0.5)*theta
          - static_cast<Scalar>(1./24.)*theta*theta_sq;
    } else {
      sin_theta_by_theta = so2.unit_complex().y()/theta;
      one_minus_cos_theta_by_theta
          = (static_cast<Scalar>(1.) - so2.unit_complex().x())/theta;
    }
    Matrix<Scalar,2,1> trans
        (sin_theta_by_theta*a[0] - one_minus_cos_theta_by_theta*a[1],
        one_minus_cos_theta_by_theta * a[0]+sin_theta_by_theta*a[1]);
    return SE2Group<Scalar>(so2, trans);
  }

  /**
   * \brief Generators
   *
   * \pre \f$ i \in \{0,1,2\} \f$
   * \returns \f$ i \f$th generator \f$ G_i \f$ of SE2
   *
   * The infinitesimal generators of SE2 are: \f[
   *        G_0 = \left( \begin{array}{ccc}
   *                          0&  0&  1\\
   *                          0&  0&  0\\
   *                          0&  0&  0\\
   *                     \end{array} \right),
   *        G_1 = \left( \begin{array}{cccc}
   *                          0&  0&  0\\
   *                          0&  0&  1\\
   *                          0&  0&  0\\
   *                     \end{array} \right),
   *        G_2 = \left( \begin{array}{cccc}
   *                          0&  0&  0&\\
   *                          0&  0& -1&\\
   *                          0&  1&  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 SE2 is defined
   * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{2\times 2},
   * \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 & v) {
    Transformation Omega;
    Omega.setZero();
    Omega.template topLeftCorner<2,2>() = SO2Group<Scalar>::hat(v[2]);
    Omega.col(2).template head<2>() = v.template head<2>();
    return Omega;
  }

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

    return Tangent(-theta1*upsilon2[1] + theta2*upsilon1[1],
        theta1*upsilon2[0] - theta2*upsilon1[0],
        static_cast<Scalar>(0));
  }

  /**
   * \brief Logarithmic map
   *
   * \param other element of the group SE2
   * \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 SE2.
   *
   * \see exp()
   * \see vee()
   */
  inline static
  const Tangent log(const SE2Group<Scalar> & other) {
    Tangent upsilon_theta;
    const SO2Group<Scalar> & so2 = other.so2();
    Scalar theta = SO2Group<Scalar>::log(so2);
    upsilon_theta[2] = theta;
    Scalar halftheta = static_cast<Scalar>(0.5)*theta;
    Scalar halftheta_by_tan_of_halftheta;

    const Matrix<Scalar,2,1> & z = so2.unit_complex();
    Scalar real_minus_one = z.x()-static_cast<Scalar>(1.);
    if (std::abs(real_minus_one)<SophusConstants<Scalar>::epsilon()) {
      halftheta_by_tan_of_halftheta
          = static_cast<Scalar>(1.)
          - static_cast<Scalar>(1./12)*theta*theta;
    } else {
      halftheta_by_tan_of_halftheta
          = -(halftheta*z.y())/(real_minus_one);
    }
    Matrix<Scalar,2,2> V_inv;
    V_inv <<  halftheta_by_tan_of_halftheta,                      halftheta
        ,                        -halftheta,  halftheta_by_tan_of_halftheta;
    upsilon_theta.template head<2>() = V_inv*other.translation();
    return upsilon_theta;
  }

  /**
   * \brief vee-operator
   *
   * \param Omega 3x3-matrix representation of Lie algebra element
   * \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) {
    Tangent upsilon_omega;
    upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
    upsilon_omega[2]
        = SO2Group<Scalar>::vee(Omega.template topLeftCorner<2,2>());
    return upsilon_omega;
  }
};

/**
 * \brief SE2 default type - Constructors and default storage for SE2 Type
 */
template<typename _Scalar, int _Options>
class SE2Group : public SE2GroupBase<SE2Group<_Scalar,_Options> > {
  typedef SE2GroupBase<SE2Group<_Scalar,_Options> > Base;

public:
  /** \brief scalar type */
  typedef typename internal::traits<SE2Group<_Scalar,_Options> >
  ::Scalar Scalar;
  /** \brief translation reference type */
  typedef typename internal::traits<SE2Group<_Scalar,_Options> >
  ::TranslationType & TranslationReference;
  typedef const typename internal::traits<SE2Group<_Scalar,_Options> >
  ::TranslationType & ConstTranslationReference;
  /** \brief SO2 reference type */
  typedef typename internal::traits<SE2Group<_Scalar,_Options> >
  ::SO2Type & SO2Reference;
  /** \brief SO2 const reference type */
  typedef const typename internal::traits<SE2Group<_Scalar,_Options> >
  ::SO2Type & ConstSO2Reference;

  /** \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 Complex to identity rotation and translation to zero.
   */
  inline
  SE2Group()
    : translation_( Matrix<Scalar,2,1>::Zero() )
  {
  }

  /**
   * \brief Copy constructor
   */
  template<typename OtherDerived> inline
  SE2Group(const SE2GroupBase<OtherDerived> & other)
    : so2_(other.so2()), translation_(other.translation()) {
  }

  /**
   * \brief Constructor from SO2 and translation vector
   */
  template<typename OtherDerived> inline
  SE2Group(const SO2GroupBase<OtherDerived> & so2,
           const Point & translation)
    : so2_(so2), translation_(translation) {
  }

  /**
   * \brief Constructor from rotation matrix and translation vector
   *
   * \pre rotation matrix need to be orthogonal with determinant of 1
   */
  inline
  SE2Group(const typename SO2Group<Scalar>::Transformation & rotation_matrix,
           const Point & translation)
    : so2_(rotation_matrix), translation_(translation) {
  }

  /**
   * \brief Constructor from rotation angle and translation vector
   */
  inline
  SE2Group(const Scalar & theta,
           const Point & translation)
    : so2_(theta), translation_(translation) {
  }

  /**
   * \brief Constructor from complex number and translation vector
   *
   * \pre complex must not be zero
   */
  inline
  SE2Group(const std::complex<Scalar> & complex,
           const Point & translation)
    : so2_(complex), translation_(translation) {
  }

  /**
   * \brief Constructor from 3x3 matrix
   *
   * \pre 2x2 sub-matrix need to be orthogonal with determinant of 1
   */
  inline explicit
  SE2Group(const Transformation & T)
    : so2_(T.template topLeftCorner<2,2>()),
      translation_(T.template block<2,1>(0,2)) {
  }

  /**
   * \returns pointer to internal data
   *
   * This provides unsafe read/write access to internal data. SE2 is represented
   * by a pair of an SO2 element (two parameters) and a translation vector (two
   * parameters). The user needs to take care of that the complex
   * stays normalized.
   *
   * /see normalize()
   */
  EIGEN_STRONG_INLINE
  Scalar* data() {
    // so2_ and translation_ are layed out sequentially with no padding
    return so2_.data();
  }

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

  /**
   * \brief Accessor of SO2
   */
  EIGEN_STRONG_INLINE
  SO2Reference so2() {
    return so2_;
  }

  /**
   * \brief Mutator of SO2
   */
  EIGEN_STRONG_INLINE
  ConstSO2Reference so2() const {
    return so2_;
  }

  /**
   * \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::SO2Group<Scalar> so2_;
  Matrix<Scalar,2,1> translation_;
};


} // end namespace


namespace Eigen {
/**
 * \brief Specialisation of Eigen::Map for SE2GroupBase
 *
 * Allows us to wrap SE2 Objects around POD array
 * (e.g. external c style complex)
 */
template<typename _Scalar, int _Options>
class Map<Sophus::SE2Group<_Scalar>, _Options>
    : public Sophus::SE2GroupBase<Map<Sophus::SE2Group<_Scalar>, _Options> >
{
  typedef Sophus::SE2GroupBase<Map<Sophus::SE2Group<_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 reference type */
  typedef const typename internal::traits<Map>::TranslationType &
  ConstTranslationReference;
  /** \brief SO2 reference type */
  typedef typename internal::traits<Map>::SO2Type & SO2Reference;
  /** \brief SO2 const reference type */
  typedef const typename internal::traits<Map>::SO2Type & ConstSO2Reference;

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

  /**
   * \brief Mutator of SO2
   */
  EIGEN_STRONG_INLINE
  SO2Reference so2() {
    return so2_;
  }

  /**
   * \brief Accessor of SO2
   */
  EIGEN_STRONG_INLINE
  ConstSO2Reference so2() const {
    return so2_;
  }

  /**
   * \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::SO2Group<Scalar>,_Options> so2_;
  Map<Matrix<Scalar,2,1>,_Options> translation_;
};

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

public:
  /** \brief scalar type */
  typedef typename internal::traits<Map>::Scalar Scalar;
  /** \brief translation reference type */
  typedef const typename internal::traits<Map>::TranslationType &
  ConstTranslationReference;
  /** \brief SO2 const reference type */
  typedef const typename internal::traits<Map>::SO2Type & ConstSO2Reference;

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

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

  /**
   * \brief Accessor of SO2
   */
  EIGEN_STRONG_INLINE
  ConstSO2Reference so2() const {
    return so2_;
  }

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

protected:
  const Map<const Sophus::SO2Group<Scalar>,_Options> so2_;
  const Map<const Matrix<Scalar,2,1>,_Options> translation_;
};

}

#endif