v1

thirdparty/Sophus/sophus/so3.hpp

@@ -0,0 +1,811 @@
+// 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