v1

thirdparty/Sophus/CMakeLists.txt

@@ -0,0 +1,83 @@
+SET(PROJECT_NAME Sophus)
+
+PROJECT(${PROJECT_NAME})
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+SET (CMAKE_VERBOSE_MAKEFILE ON)
+
+# Release by default
+# Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug"
+IF( NOT CMAKE_BUILD_TYPE )
+   SET( CMAKE_BUILD_TYPE Release )
+ENDIF()
+
+IF (CMAKE_COMPILER_IS_GNUCXX )
+   SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
+   SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ")
+
+  ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable
+                   -Wno-unused-but-set-variable -Wno-unknown-pragmas ")
+ENDIF()
+
+################################################################################
+# Add local path for finding packages, set the local version first
+set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" )
+list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" )
+
+################################################################################
+# Create variables used for exporting in SophusConfig.cmake
+set( Sophus_LIBRARIES "" )
+set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} )
+
+################################################################################
+
+include(FindEigen3.cmake)
+#find_package( Eigen3 REQUIRED )
+INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} )
+SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} )
+
+SET (SOURCE_DIR "sophus")
+
+SET (TEMPLATES tests
+               so2
+               se2
+               so3
+               se3
+               rxso3
+               sim3
+)
+
+SET (SOURCES ${SOURCE_DIR}/sophus.hpp)
+
+FOREACH(templ ${TEMPLATES})
+  LIST(APPEND SOURCES ${SOURCE_DIR}/${templ}.hpp)
+ENDFOREACH(templ)
+
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+# Added ${SOURCES} to executables so they show up in QtCreator (and possibly
+# other IDEs).
+# ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp ${SOURCES})
+# ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp ${SOURCES})
+# ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp ${SOURCES})
+# ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp ${SOURCES})
+# ADD_EXECUTABLE(test_rxso3 sophus/test_rxso3.cpp ${SOURCES})
+# ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp ${SOURCES})
+# ENABLE_TESTING()
+# 
+# ADD_TEST(test_so2 test_so2)
+# ADD_TEST(test_se2 test_se2)
+# ADD_TEST(test_so3 test_so3)
+# ADD_TEST(test_se3 test_se3)
+# ADD_TEST(test_rxso3 test_rxso3)
+# ADD_TEST(test_sim3 test_sim3)
+
+################################################################################
+# Create the SophusConfig.cmake file for other cmake projects.
+CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in
+    ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE )
+export( PACKAGE Sophus )
+
+INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include
+        FILES_MATCHING PATTERN "*.hpp" )

thirdparty/Sophus/FindEigen3.cmake

@@ -0,0 +1,81 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

thirdparty/Sophus/README

@@ -0,0 +1,19 @@
+Sophus (version 0.9a)
+
+C++ implementation of Lie Groups using Eigen.
+
+Thanks to Steven Lovegrove, Sophus is now fully templated  - using the Curiously Recurring Template Pattern (CRTP).
+
+(In order to go back to the non-templated/double-only version "git checkout a621ff".)
+
+Installation guide:
+
+>>>
+cd Sophus
+mkdir build
+cd build
+cmake ..
+make
+<<<
+
+

thirdparty/Sophus/SophusConfig.cmake.in

@@ -0,0 +1,11 @@
+################################################################################
+# Sophus source dir
+set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@")
+
+################################################################################
+# Sophus build dir
+set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@")
+
+################################################################################
+set( Sophus_INCLUDE_DIR  "@Sophus_INCLUDE_DIR@" )
+set( Sophus_INCLUDE_DIRS  "@Sophus_INCLUDE_DIR@" )

thirdparty/Sophus/cmake_modules/FindEigen3.cmake

@@ -0,0 +1,26 @@
+# - Try to find Eigen3 lib
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+
+if( EIGEN3_INCLUDE_DIR )
+    # in cache already
+    set( EIGEN3_FOUND TRUE )
+else (EIGEN3_INCLUDE_DIR)
+    find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core
+        PATH_SUFFIXES eigen3/
+        HINTS
+        ${INCLUDE_INSTALL_DIR}
+        /usr/local/include
+        ${KDE4_INCLUDE_DIR}
+        )
+    include( FindPackageHandleStandardArgs )
+    find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR )   
+    mark_as_advanced( EIGEN3_INCLUDE_DIR )
+endif(EIGEN3_INCLUDE_DIR)
+

thirdparty/Sophus/sophus/rxso3.hpp

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

thirdparty/Sophus/sophus/se2.hpp

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

thirdparty/Sophus/sophus/se3.hpp

@@ -0,0 +1,947 @@
+// This file is part of Sophus.
+//
+// Copyright 2011-2013 Hauke Strasdat
+//           2012-2013 Steven Lovegrove
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#ifndef SOPHUS_SE3_HPP
+#define SOPHUS_SE3_HPP
+
+#include "so3.hpp"
+
+////////////////////////////////////////////////////////////////////////////
+// Forward Declarations / typedefs
+////////////////////////////////////////////////////////////////////////////
+
+namespace Sophus {
+template<typename _Scalar, int _Options=0> class SE3Group;
+typedef SE3Group<double> SE3 EIGEN_DEPRECATED;
+typedef SE3Group<double> SE3d; /**< double precision SE3 */
+typedef SE3Group<float> SE3f;  /**< single precision SE3 */
+typedef Matrix<double,6,1> Vector6d;
+typedef Matrix<double,6,6> Matrix6d;
+typedef Matrix<float,6,1> Vector6f;
+typedef Matrix<float,6,6> Matrix6f;
+}
+
+////////////////////////////////////////////////////////////////////////////
+// Eigen Traits (For querying derived types in CRTP hierarchy)
+////////////////////////////////////////////////////////////////////////////
+
+namespace Eigen {
+namespace internal {
+
+template<typename _Scalar, int _Options>
+struct traits<Sophus::SE3Group<_Scalar,_Options> > {
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,3,1> TranslationType;
+  typedef Sophus::SO3Group<Scalar> SO3Type;
+};
+
+template<typename _Scalar, int _Options>
+struct traits<Map<Sophus::SE3Group<_Scalar>, _Options> >
+    : traits<Sophus::SE3Group<_Scalar, _Options> > {
+  typedef _Scalar Scalar;
+  typedef Map<Matrix<Scalar,3,1>,_Options> TranslationType;
+  typedef Map<Sophus::SO3Group<Scalar>,_Options> SO3Type;
+};
+
+template<typename _Scalar, int _Options>
+struct traits<Map<const Sophus::SE3Group<_Scalar>, _Options> >
+    : traits<const Sophus::SE3Group<_Scalar, _Options> > {
+  typedef _Scalar Scalar;
+  typedef Map<const Matrix<Scalar,3,1>,_Options> TranslationType;
+  typedef Map<const Sophus::SO3Group<Scalar>,_Options> SO3Type;
+};
+
+}
+}
+
+namespace Sophus {
+using namespace Eigen;
+using namespace std;
+
+/**
+ * \brief SE3 base type - implements SE3 class but is storage agnostic
+ *
+ * [add more detailed description/tutorial]
+ */
+template<typename Derived>
+class SE3GroupBase {
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  /** \brief translation reference type */
+  typedef typename internal::traits<Derived>::TranslationType &
+  TranslationReference;
+  /** \brief translation const reference type */
+  typedef const typename internal::traits<Derived>::TranslationType &
+  ConstTranslationReference;
+  /** \brief SO3 reference type */
+  typedef typename internal::traits<Derived>::SO3Type &
+  SO3Reference;
+  /** \brief SO3 const reference type */
+  typedef const typename internal::traits<Derived>::SO3Type &
+  ConstSO3Reference;
+
+  /** \brief degree of freedom of group
+    *        (three for translation, three for rotation) */
+  static const int DoF = 6;
+  /** \brief number of internal parameters used
+   *         (unit quaternion for rotation + translation 3-vector) */
+  static const int num_parameters = 7;
+  /** \brief group transformations are NxN matrices */
+  static const int N = 4;
+  /** \brief group transfomation type */
+  typedef Matrix<Scalar,N,N> Transformation;
+  /** \brief point type */
+  typedef Matrix<Scalar,3,1> Point;
+  /** \brief tangent vector type */
+  typedef Matrix<Scalar,DoF,1> Tangent;
+  /** \brief adjoint transformation type */
+  typedef Matrix<Scalar,DoF,DoF> Adjoint;
+
+
+  /**
+   * \brief Adjoint transformation
+   *
+   * This function return the adjoint transformation \f$ Ad \f$ of the
+   * group instance \f$ A \f$  such that for all \f$ x \f$
+   * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
+   * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
+   */
+  inline
+  const Adjoint Adj() const {
+    const Matrix<Scalar,3,3> & R = so3().matrix();
+    Adjoint res;
+    res.block(0,0,3,3) = R;
+    res.block(3,3,3,3) = R;
+    res.block(0,3,3,3) = SO3Group<Scalar>::hat(translation())*R;
+    res.block(3,0,3,3) = Matrix<Scalar,3,3>::Zero(3,3);
+    return res;
+  }
+
+  /**
+   * \returns copy of instance casted to NewScalarType
+   */
+  template<typename NewScalarType>
+  inline SE3Group<NewScalarType> cast() const {
+    return
+        SE3Group<NewScalarType>(so3().template cast<NewScalarType>(),
+                                translation().template cast<NewScalarType>() );
+  }
+
+  /**
+   * \brief Fast group multiplication
+   *
+   * This method is a fast version of operator*=(), since it does not perform
+   * normalization. It is up to the user to call normalize() once in a while.
+   *
+   * \see operator*=()
+   */
+  inline
+  void fastMultiply(const SE3Group<Scalar>& other) {
+    translation() += so3()*(other.translation());
+    so3().fastMultiply(other.so3());
+  }
+
+  /**
+   * \returns Group inverse of instance
+   */
+  inline
+  const SE3Group<Scalar> inverse() const {
+    const SO3Group<Scalar> invR = so3().inverse();
+    return SE3Group<Scalar>(invR, invR*(translation()
+                                        *static_cast<Scalar>(-1) ) );
+  }
+
+  /**
+   * \brief Logarithmic map
+   *
+   * \returns tangent space representation
+   *          (translational part and rotation vector) of instance
+   *
+   * \see  log().
+   */
+  inline
+  const Tangent log() const {
+    return log(*this);
+  }
+
+  /**
+   * \brief Normalize SO3 element
+   *
+   * It re-normalizes the SO3 element. This method only needs to
+   * be called in conjunction with fastMultiply() or data() write access.
+   */
+  inline
+  void normalize() {
+    so3().normalize();
+  }
+
+  /**
+   * \returns 4x4 matrix representation of instance
+   */
+  inline
+  const Transformation matrix() const {
+    Transformation homogenious_matrix;
+    homogenious_matrix.setIdentity();
+    homogenious_matrix.block(0,0,3,3) = rotationMatrix();
+    homogenious_matrix.col(3).head(3) = translation();
+    return homogenious_matrix;
+  }
+
+  /**
+   * \returns 3x4 matrix representation of instance
+   *
+   * It returns the three first row of matrix().
+   */
+  inline
+  const Matrix<Scalar,3,4> matrix3x4() const {
+    Matrix<Scalar,3,4> matrix;
+    matrix.block(0,0,3,3) = rotationMatrix();
+    matrix.col(3) = translation();
+    return matrix;
+  }
+
+  /**
+   * \brief Assignment operator
+   */
+  template<typename OtherDerived> inline
+  SE3GroupBase<Derived>& operator= (const SE3GroupBase<OtherDerived> & other) {
+    so3() = other.so3();
+    translation() = other.translation();
+    return *this;
+  }
+
+  /**
+   * \brief Group multiplication
+   * \see operator*=()
+   */
+  inline
+  const SE3Group<Scalar> operator*(const SE3Group<Scalar>& other) const {
+    SE3Group<Scalar> result(*this);
+    result *= other;
+    return result;
+  }
+
+  /**
+   * \brief Group action on \f$ \mathbf{R}^3 \f$
+   *
+   * \param p point \f$p \in \mathbf{R}^3 \f$
+   * \returns point \f$p' \in \mathbf{R}^3 \f$,
+   *          rotated and translated version of \f$p\f$
+   *
+   * This function rotates and translates point \f$ p \f$
+   * in \f$ \mathbf{R}^3 \f$ by the SE3 transformation \f$R,t\f$
+   * (=rotation matrix, translation vector): \f$ p' = R\cdot p + t \f$.
+   */
+  inline
+  const Point operator*(const Point & p) const {
+    return so3()*p + translation();
+  }
+
+  /**
+   * \brief In-place group multiplication
+   *
+   * \see fastMultiply()
+   * \see operator*()
+   */
+  inline
+  void operator*=(const SE3Group<Scalar>& other) {
+    fastMultiply(other);
+    normalize();
+  }
+
+
+  /**
+   * \returns Rotation matrix
+   *
+   * deprecated: use rotationMatrix() instead.
+   */
+  typedef Transformation M3_marcos_dont_like_commas;
+  inline
+  EIGEN_DEPRECATED const M3_marcos_dont_like_commas rotation_matrix() const {
+    return so3().matrix();
+  }
+
+  /**
+   * \returns Rotation matrix
+   */
+  inline
+  const Matrix<Scalar,3,3> rotationMatrix() const {
+    return so3().matrix();
+  }
+
+
+  /**
+   * \brief Mutator of SO3 group
+   */
+  EIGEN_STRONG_INLINE
+  SO3Reference so3() {
+    return static_cast<Derived*>(this)->so3();
+  }
+
+  /**
+   * \brief Accessor of SO3 group
+   */
+  EIGEN_STRONG_INLINE
+  ConstSO3Reference so3() const {
+    return static_cast<const Derived*>(this)->so3();
+  }
+
+  /**
+   * \brief Setter of internal unit quaternion representation
+   *
+   * \param quaternion
+   * \pre   the quaternion must not be zero
+   *
+   * The quaternion is normalized to unit length.
+   */
+  inline
+  void setQuaternion(const Quaternion<Scalar> & quat) {
+    return so3().setQuaternion(quat);
+  }
+
+  /**
+   * \brief Setter of unit quaternion using rotation matrix
+   *
+   * \param rotation_matrix a 3x3 rotation matrix
+   * \pre   the 3x3 matrix should be orthogonal and have a determinant of 1
+   */
+  inline
+  void setRotationMatrix
+  (const Matrix<Scalar,3,3> & rotation_matrix) {
+    so3().setQuaternion(Quaternion<Scalar>(rotation_matrix));
+  }
+
+  /**
+   * \brief Mutator of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  TranslationReference translation() {
+    return static_cast<Derived*>(this)->translation();
+  }
+
+  /**
+   * \brief Accessor of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  ConstTranslationReference translation() const {
+    return static_cast<const Derived*>(this)->translation();
+  }
+
+  /**
+   * \brief Accessor of unit quaternion
+   *
+   * No direct write access is given to ensure the quaternion stays normalized.
+   */
+  inline
+  typename internal::traits<Derived>::SO3Type::ConstQuaternionReference
+  unit_quaternion() const {
+    return so3().unit_quaternion();
+  }
+
+  ////////////////////////////////////////////////////////////////////////////
+  // public static functions
+  ////////////////////////////////////////////////////////////////////////////
+
+  /**
+   * \param   b 6-vector representation of Lie algebra element
+   * \returns   derivative of Lie bracket
+   *
+   * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{se3} \f$
+   * with \f$ [a, b]_{se3} \f$ being the lieBracket() of the Lie algebra se3.
+   *
+   * \see lieBracket()
+   */
+  inline static
+  const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
+    Adjoint res;
+    res.setZero();
+
+    const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
+    const Matrix<Scalar,3,1> & omega2 = b.template tail<3>();
+
+    res.template topLeftCorner<3,3>() = -SO3Group<Scalar>::hat(omega2);
+    res.template topRightCorner<3,3>() = -SO3Group<Scalar>::hat(upsilon2);
+    res.template bottomRightCorner<3,3>() = -SO3Group<Scalar>::hat(omega2);
+    return res;
+  }
+
+  /**
+   * \brief Group exponential
+   *
+   * \param a tangent space element (6-vector)
+   * \returns corresponding element of the group SE3
+   *
+   * The first three components of \f$ a \f$ represent the translational
+   * part \f$ \upsilon \f$ in the tangent space of SE3, while the last three
+   * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$.
+   *
+   * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
+   * with \f$ \exp(\cdot) \f$ being the matrix exponential
+   * and \f$ \widehat{\cdot} \f$ the hat()-operator of SE3.
+   *
+   * \see hat()
+   * \see log()
+   */
+  inline static
+  const SE3Group<Scalar> exp(const Tangent & a) {
+    const Matrix<Scalar,3,1> & omega = a.template tail<3>();
+
+    Scalar theta;
+    const SO3Group<Scalar> & so3
+        = SO3Group<Scalar>::expAndTheta(omega, &theta);
+
+    const Matrix<Scalar,3,3> & Omega = SO3Group<Scalar>::hat(omega);
+    const Matrix<Scalar,3,3> & Omega_sq = Omega*Omega;
+    Matrix<Scalar,3,3> V;
+
+    if(theta<SophusConstants<Scalar>::epsilon()) {
+      V = so3.matrix();
+      //Note: That is an accurate expansion!
+    } else {
+      Scalar theta_sq = theta*theta;
+      V = (Matrix<Scalar,3,3>::Identity()
+           + (static_cast<Scalar>(1)-std::cos(theta))/(theta_sq)*Omega
+           + (theta-std::sin(theta))/(theta_sq*theta)*Omega_sq);
+    }
+    return SE3Group<Scalar>(so3,V*a.template head<3>());
+  }
+
+  /**
+   * \brief Generators
+   *
+   * \pre \f$ i \in \{0,1,2,3,4,5\} \f$
+   * \returns \f$ i \f$th generator \f$ G_i \f$ of SE3
+   *
+   * The infinitesimal generators of SE3 are: \f[
+   *        G_0 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  1\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_1 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  1\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_2 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  1\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right).
+   *        G_3 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  0\\
+   *                          0&  0& -1&  0\\
+   *                          0&  1&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_4 = \left( \begin{array}{cccc}
+   *                          0&  0&  1&  0\\
+   *                          0&  0&  0&  0\\
+   *                         -1&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_5 = \left( \begin{array}{cccc}
+   *                          0& -1&  0&  0\\
+   *                          1&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right).
+   * \f]
+   * \see hat()
+   */
+  inline static
+  const Transformation generator(int i) {
+    if (i<0 || i>5) {
+      throw SophusException("i is not in range [0,5].");
+    }
+    Tangent e;
+    e.setZero();
+    e[i] = static_cast<Scalar>(1);
+    return hat(e);
+  }
+
+  /**
+   * \brief hat-operator
+   *
+   * \param omega 6-vector representation of Lie algebra element
+   * \returns     4x4-matrix representatin of Lie algebra element
+   *
+   * Formally, the hat-operator of SE3 is defined
+   * as \f$ \widehat{\cdot}: \mathbf{R}^6 \rightarrow \mathbf{R}^{4\times 4},
+   * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$
+   * with \f$ G_i \f$ being the ith infinitesial generator().
+   *
+   * \see generator()
+   * \see vee()
+   */
+  inline static
+  const Transformation hat(const Tangent & v) {
+    Transformation Omega;
+    Omega.setZero();
+    Omega.template topLeftCorner<3,3>()
+        = SO3Group<Scalar>::hat(v.template tail<3>());
+    Omega.col(3).template head<3>() = v.template head<3>();
+    return Omega;
+  }
+
+  /**
+   * \brief Lie bracket
+   *
+   * \param a 6-vector representation of Lie algebra element
+   * \param b 6-vector representation of Lie algebra element
+   * \returns      6-vector representation of Lie algebra element
+   *
+   * It computes the bracket of SE3. To be more specific, it
+   * computes \f$ [a, b]_{se3}
+   * := [\widehat{a}, \widehat{b}]^\vee \f$
+   * with \f$ [A,B] = AB-BA \f$ being the matrix
+   * commutator, \f$ \widehat{\cdot} \f$ the
+   * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SE3.
+   *
+   * \see hat()
+   * \see vee()
+   */
+  inline static
+  const Tangent lieBracket(const Tangent & a,
+                           const Tangent & b) {
+    Matrix<Scalar,3,1> upsilon1 = a.template head<3>();
+    Matrix<Scalar,3,1> upsilon2 = b.template head<3>();
+    Matrix<Scalar,3,1> omega1 = a.template tail<3>();
+    Matrix<Scalar,3,1> omega2 = b.template tail<3>();
+
+    Tangent res;
+    res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2);
+    res.template tail<3>() = omega1.cross(omega2);
+
+    return res;
+  }
+
+  /**
+   * \brief Logarithmic map
+   *
+   * \param other element of the group SE3
+   * \returns     corresponding tangent space element
+   *              (translational part \f$ \upsilon \f$
+   *               and rotation vector \f$ \omega \f$)
+   *
+   * Computes the logarithmic, the inverse of the group exponential.
+   * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
+   * with \f$ \vee(\cdot) \f$ being the matrix logarithm
+   * and \f$ \vee{\cdot} \f$ the vee()-operator of SE3.
+   *
+   * \see exp()
+   * \see vee()
+   */
+  inline static
+  const Tangent log(const SE3Group<Scalar> & se3) {
+    Tangent upsilon_omega;
+    Scalar theta;
+    upsilon_omega.template tail<3>()
+        = SO3Group<Scalar>::logAndTheta(se3.so3(), &theta);
+
+    if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
+      const Matrix<Scalar,3,3> & Omega
+          = SO3Group<Scalar>::hat(upsilon_omega.template tail<3>());
+      const Matrix<Scalar,3,3> & V_inv =
+          Matrix<Scalar,3,3>::Identity() -
+          static_cast<Scalar>(0.5)*Omega
+          + static_cast<Scalar>(1./12.)*(Omega*Omega);
+
+      upsilon_omega.template head<3>() = V_inv*se3.translation();
+    } else {
+      const Matrix<Scalar,3,3> & Omega
+          = SO3Group<Scalar>::hat(upsilon_omega.template tail<3>());
+      const Matrix<Scalar,3,3> & V_inv =
+          ( Matrix<Scalar,3,3>::Identity() - static_cast<Scalar>(0.5)*Omega
+            + ( static_cast<Scalar>(1)
+                - theta/(static_cast<Scalar>(2)*tan(theta/Scalar(2)))) /
+            (theta*theta)*(Omega*Omega) );
+      upsilon_omega.template head<3>() = V_inv*se3.translation();
+    }
+    return upsilon_omega;
+  }
+
+  /**
+   * \brief vee-operator
+   *
+   * \param Omega 4x4-matrix representation of Lie algebra element
+   * \returns     6-vector representatin of Lie algebra element
+   *
+   * This is the inverse of the hat()-operator.
+   *
+   * \see hat()
+   */
+  inline static
+  const Tangent vee(const Transformation & Omega) {
+    Tangent upsilon_omega;
+    upsilon_omega.template head<3>() = Omega.col(3).template head<3>();
+    upsilon_omega.template tail<3>()
+        = SO3Group<Scalar>::vee(Omega.template topLeftCorner<3,3>());
+    return upsilon_omega;
+  }
+};
+
+/**
+ * \brief SE3 default type - Constructors and default storage for SE3 Type
+ */
+template<typename _Scalar, int _Options>
+class SE3Group : public SE3GroupBase<SE3Group<_Scalar,_Options> > {
+  typedef SE3GroupBase<SE3Group<_Scalar,_Options> > Base;
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<SE3Group<_Scalar,_Options> >
+  ::Scalar Scalar;
+  /** \brief SO3 reference type */
+  typedef typename internal::traits<SE3Group<_Scalar,_Options> >
+  ::SO3Type & SO3Reference;
+  /** \brief SO3 const reference type */
+  typedef const typename internal::traits<SE3Group<_Scalar,_Options> >
+  ::SO3Type & ConstSO3Reference;
+  /** \brief translation reference type */
+  typedef typename internal::traits<SE3Group<_Scalar,_Options> >
+  ::TranslationType & TranslationReference;
+  /** \brief translation const reference type */
+  typedef const typename internal::traits<SE3Group<_Scalar,_Options> >
+  ::TranslationType & ConstTranslationReference;
+
+  /** \brief degree of freedom of group */
+  static const int DoF = Base::DoF;
+  /** \brief number of internal parameters used */
+  static const int num_parameters = Base::num_parameters;
+  /** \brief group transformations are NxN matrices */
+  static const int N = Base::N;
+  /** \brief group transfomation type */
+  typedef typename Base::Transformation Transformation;
+  /** \brief point type */
+  typedef typename Base::Point Point;
+  /** \brief tangent vector type */
+  typedef typename Base::Tangent Tangent;
+  /** \brief adjoint transformation type */
+  typedef typename Base::Adjoint Adjoint;
+
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * \brief Default constructor
+   *
+   * Initialize Quaternion to identity rotation and translation to zero.
+   */
+  inline
+  SE3Group()
+    : translation_( Matrix<Scalar,3,1>::Zero() )
+  {
+  }
+
+  /**
+   * \brief Copy constructor
+   */
+  template<typename OtherDerived> inline
+  SE3Group(const SE3GroupBase<OtherDerived> & other)
+    : so3_(other.so3()), translation_(other.translation()) {
+  }
+
+  /**
+   * \brief Constructor from SO3 and translation vector
+   */
+  template<typename OtherDerived> inline
+  SE3Group(const SO3GroupBase<OtherDerived> & so3,
+           const Point & translation)
+    : so3_(so3), translation_(translation) {
+  }
+
+  /**
+   * \brief Constructor from rotation matrix and translation vector
+   *
+   * \pre rotation matrix need to be orthogonal with determinant of 1
+   */
+  inline
+  SE3Group(const Matrix<Scalar,3,3> & rotation_matrix,
+           const Point & translation)
+    : so3_(rotation_matrix), translation_(translation) {
+  }
+
+  /**
+   * \brief Constructor from quaternion and translation vector
+   *
+   * \pre quaternion must not be zero
+   */
+  inline
+  SE3Group(const Quaternion<Scalar> & quaternion,
+           const Point & translation)
+    : so3_(quaternion), translation_(translation) {
+  }
+
+  /**
+   * \brief Constructor from 4x4 matrix
+   *
+   * \pre top-left 3x3 sub-matrix need to be orthogonal with determinant of 1
+   */
+  inline explicit
+  SE3Group(const Eigen::Matrix<Scalar,4,4>& T)
+    : so3_(T.template topLeftCorner<3,3>()),
+      translation_(T.template block<3,1>(0,3)) {
+  }
+
+  /**
+   * \returns pointer to internal data
+   *
+   * This provides unsafe read/write access to internal data. SE3 is represented
+   * by a pair of an SO3 element (4 parameters) and translation vector (three
+   * parameters). The user needs to take care of that the quaternion
+   * stays normalized.
+   *
+   * Note: The first three Scalars represent the imaginary parts, while the
+   * forth Scalar represent the real part.
+   *
+   * /see normalize()
+   */
+  EIGEN_STRONG_INLINE
+  Scalar* data() {
+    // so3_ and translation_ are layed out sequentially with no padding
+    return so3_.data();
+  }
+
+  /**
+   * \returns const pointer to internal data
+   *
+   * Const version of data().
+   */
+  EIGEN_STRONG_INLINE
+  const Scalar* data() const {
+    // so3_ and translation_ are layed out sequentially with no padding
+    return so3_.data();
+  }
+
+  /**
+   * \brief Accessor of SO3
+   */
+  EIGEN_STRONG_INLINE
+  SO3Reference so3() {
+    return so3_;
+  }
+
+  /**
+   * \brief Mutator of SO3
+   */
+  EIGEN_STRONG_INLINE
+  ConstSO3Reference so3() const {
+    return so3_;
+  }
+
+  /**
+   * \brief Mutator of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  TranslationReference translation() {
+    return translation_;
+  }
+
+  /**
+   * \brief Accessor of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  ConstTranslationReference translation() const {
+    return translation_;
+  }
+
+protected:
+  Sophus::SO3Group<Scalar> so3_;
+  Matrix<Scalar,3,1> translation_;
+};
+
+
+} // end namespace
+
+
+namespace Eigen {
+/**
+ * \brief Specialisation of Eigen::Map for SE3GroupBase
+ *
+ * Allows us to wrap SE3 Objects around POD array
+ * (e.g. external c style quaternion)
+ */
+template<typename _Scalar, int _Options>
+class Map<Sophus::SE3Group<_Scalar>, _Options>
+    : public Sophus::SE3GroupBase<Map<Sophus::SE3Group<_Scalar>, _Options> > {
+  typedef Sophus::SE3GroupBase<Map<Sophus::SE3Group<_Scalar>, _Options> > Base;
+
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Map>::Scalar Scalar;
+  /** \brief translation reference type */
+  typedef typename internal::traits<Map>::TranslationType &
+  TranslationReference;
+  /** \brief translation const reference type */
+  typedef const typename internal::traits<Map>::TranslationType &
+  ConstTranslationReference;
+  /** \brief SO3 reference type */
+  typedef typename internal::traits<Map>::SO3Type &
+  SO3Reference;
+  /** \brief SO3 const reference type */
+  typedef const typename internal::traits<Map>::SO3Type &
+  ConstSO3Reference;
+
+  /** \brief degree of freedom of group */
+  static const int DoF = Base::DoF;
+  /** \brief number of internal parameters used */
+  static const int num_parameters = Base::num_parameters;
+  /** \brief group transformations are NxN matrices */
+  static const int N = Base::N;
+  /** \brief group transfomation type */
+  typedef typename Base::Transformation Transformation;
+  /** \brief point type */
+  typedef typename Base::Point Point;
+  /** \brief tangent vector type */
+  typedef typename Base::Tangent Tangent;
+  /** \brief adjoint transformation type */
+  typedef typename Base::Adjoint Adjoint;
+
+  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
+  using Base::operator*=;
+  using Base::operator*;
+
+  EIGEN_STRONG_INLINE
+  Map(Scalar* coeffs)
+    : so3_(coeffs),
+      translation_(coeffs+Sophus::SO3Group<Scalar>::num_parameters) {
+  }
+
+  /**
+   * \brief Mutator of SO3
+   */
+  EIGEN_STRONG_INLINE
+  SO3Reference so3() {
+    return so3_;
+  }
+
+  /**
+   * \brief Accessor of SO3
+   */
+  EIGEN_STRONG_INLINE
+  ConstSO3Reference so3() const {
+    return so3_;
+  }
+
+  /**
+   * \brief Mutator of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  TranslationReference translation() {
+    return translation_;
+  }
+
+  /**
+   * \brief Accessor of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  ConstTranslationReference translation() const {
+    return translation_;
+  }
+
+protected:
+  Map<Sophus::SO3Group<Scalar>,_Options> so3_;
+  Map<Matrix<Scalar,3,1>,_Options> translation_;
+};
+
+/**
+ * \brief Specialisation of Eigen::Map for const SE3GroupBase
+ *
+ * Allows us to wrap SE3 Objects around POD array
+ * (e.g. external c style quaternion)
+ */
+template<typename _Scalar, int _Options>
+class Map<const Sophus::SE3Group<_Scalar>, _Options>
+    : public Sophus::SE3GroupBase<
+    Map<const Sophus::SE3Group<_Scalar>, _Options> > {
+  typedef Sophus::SE3GroupBase<Map<const Sophus::SE3Group<_Scalar>, _Options> >
+  Base;
+
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Map>::Scalar Scalar;
+  /** \brief translation const reference type */
+  typedef const typename internal::traits<Map>::TranslationType &
+  ConstTranslationReference;
+  /** \brief SO3 const reference type */
+  typedef const typename internal::traits<Map>::SO3Type &
+  ConstSO3Reference;
+
+  /** \brief degree of freedom of group */
+  static const int DoF = Base::DoF;
+  /** \brief number of internal parameters used */
+  static const int num_parameters = Base::num_parameters;
+  /** \brief group transformations are NxN matrices */
+  static const int N = Base::N;
+  /** \brief group transfomation type */
+  typedef typename Base::Transformation Transformation;
+  /** \brief point type */
+  typedef typename Base::Point Point;
+  /** \brief tangent vector type */
+  typedef typename Base::Tangent Tangent;
+  /** \brief adjoint transformation type */
+  typedef typename Base::Adjoint Adjoint;
+
+  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
+  using Base::operator*=;
+  using Base::operator*;
+
+  EIGEN_STRONG_INLINE
+  Map(const Scalar* coeffs)
+    : so3_(coeffs),
+      translation_(coeffs+Sophus::SO3Group<Scalar>::num_parameters) {
+  }
+
+  EIGEN_STRONG_INLINE
+  Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs)
+    : translation_(trans_coeffs), so3_(rot_coeffs){
+  }
+
+  /**
+   * \brief Accessor of SO3
+   */
+  EIGEN_STRONG_INLINE
+  ConstSO3Reference so3() const {
+    return so3_;
+  }
+
+  /**
+   * \brief Accessor of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  ConstTranslationReference translation() const {
+    return translation_;
+  }
+
+protected:
+  const Map<const Sophus::SO3Group<Scalar>,_Options> so3_;
+  const Map<const Matrix<Scalar,3,1>,_Options> translation_;
+};
+
+}
+
+#endif

thirdparty/Sophus/sophus/sim3.hpp

@@ -0,0 +1,976 @@
+// 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_SIM3_HPP
+#define SOPHUS_SIM3_HPP
+
+#include "rxso3.hpp"
+
+////////////////////////////////////////////////////////////////////////////
+// Forward Declarations / typedefs
+////////////////////////////////////////////////////////////////////////////
+
+namespace Sophus {
+template<typename _Scalar, int _Options=0> class Sim3Group;
+typedef Sim3Group<double> Sim3 EIGEN_DEPRECATED;
+typedef Sim3Group<double> Sim3d; /**< double precision Sim3 */
+typedef Sim3Group<float> Sim3f;  /**< single precision Sim3 */
+typedef Matrix<double,7,1> Vector7d;
+typedef Matrix<double,7,7> Matrix7d;
+typedef Matrix<float,7,1> Vector7f;
+typedef Matrix<float,7,7> Matrix7f;
+}
+
+////////////////////////////////////////////////////////////////////////////
+// Eigen Traits (For querying derived types in CRTP hierarchy)
+////////////////////////////////////////////////////////////////////////////
+
+namespace Eigen {
+namespace internal {
+
+template<typename _Scalar, int _Options>
+struct traits<Sophus::Sim3Group<_Scalar,_Options> > {
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,3,1> TranslationType;
+  typedef Sophus::RxSO3Group<Scalar> RxSO3Type;
+};
+
+template<typename _Scalar, int _Options>
+struct traits<Map<Sophus::Sim3Group<_Scalar>, _Options> >
+    : traits<Sophus::Sim3Group<_Scalar, _Options> > {
+  typedef _Scalar Scalar;
+  typedef Map<Matrix<Scalar,3,1>,_Options> TranslationType;
+  typedef Map<Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
+};
+
+template<typename _Scalar, int _Options>
+struct traits<Map<const Sophus::Sim3Group<_Scalar>, _Options> >
+    : traits<const Sophus::Sim3Group<_Scalar, _Options> > {
+  typedef _Scalar Scalar;
+  typedef Map<const Matrix<Scalar,3,1>,_Options> TranslationType;
+  typedef Map<const Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
+};
+
+}
+}
+
+namespace Sophus {
+using namespace Eigen;
+using namespace std;
+
+/**
+ * \brief Sim3 base type - implements Sim3 class but is storage agnostic
+ *
+ * [add more detailed description/tutorial]
+ */
+template<typename Derived>
+class Sim3GroupBase {
+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 RxSO3 reference type */
+  typedef typename internal::traits<Derived>::RxSO3Type &
+  RxSO3Reference;
+  /** \brief RxSO3 const reference type */
+  typedef const typename internal::traits<Derived>::RxSO3Type &
+  ConstRxSO3Reference;
+
+
+  /** \brief degree of freedom of group
+    *        (three for translation, three for rotation, one for scale) */
+  static const int DoF = 7;
+  /** \brief number of internal parameters used
+   *         (quaternion for rotation and scale + translation 3-vector) */
+  static const int num_parameters = 7;
+  /** \brief group transformations are NxN matrices */
+  static const int N = 4;
+  /** \brief group transfomation type */
+  typedef Matrix<Scalar,N,N> Transformation;
+  /** \brief point type */
+  typedef Matrix<Scalar,3,1> Point;
+  /** \brief tangent vector type */
+  typedef Matrix<Scalar,DoF,1> Tangent;
+  /** \brief adjoint transformation type */
+  typedef Matrix<Scalar,DoF,DoF> Adjoint;
+
+  /**
+   * \brief Adjoint transformation
+   *
+   * This function return the adjoint transformation \f$ Ad \f$ of the
+   * group instance \f$ A \f$  such that for all \f$ x \f$
+   * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
+   * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
+   */
+  inline
+  const Adjoint Adj() const {
+    const Matrix<Scalar,3,3> & R = rxso3().rotationMatrix();
+    Adjoint res;
+    res.setZero();
+    res.block(0,0,3,3) = scale()*R;
+    res.block(0,3,3,3) = SO3Group<Scalar>::hat(translation())*R;
+    res.block(0,6,3,1) = -translation();
+    res.block(3,3,3,3) = R;
+    res(6,6) = 1;
+    return res;
+  }
+
+  /**
+   * \returns copy of instance casted to NewScalarType
+   */
+  template<typename NewScalarType>
+  inline Sim3Group<NewScalarType> cast() const {
+    return
+        Sim3Group<NewScalarType>(rxso3().template cast<NewScalarType>(),
+                                 translation().template cast<NewScalarType>() );
+  }
+
+  /**
+   * \brief In-place group multiplication
+   *
+   * Same as operator*=() for Sim3.
+   *
+   * \see operator*()
+   */
+  inline
+  void fastMultiply(const Sim3Group<Scalar>& other) {
+    translation() += (rxso3() * other.translation());
+    rxso3() *= other.rxso3();
+  }
+
+  /**
+   * \returns Group inverse of instance
+   */
+  inline
+  const Sim3Group<Scalar> inverse() const {
+    const RxSO3Group<Scalar> invR = rxso3().inverse();
+    return Sim3Group<Scalar>(invR, invR*(translation()
+                                         *static_cast<Scalar>(-1) ) );
+  }
+
+  /**
+   * \brief Logarithmic map
+   *
+   * \returns tangent space representation
+   *          (translational part and rotation vector) of instance
+   *
+   * \see  log().
+   */
+  inline
+  const Tangent log() const {
+    return log(*this);
+  }
+
+  /**
+   * \returns 4x4 matrix representation of instance
+   */
+  inline
+  const Transformation matrix() const {
+    Transformation homogenious_matrix;
+    homogenious_matrix.setIdentity();
+    homogenious_matrix.block(0,0,3,3) = rxso3().matrix();
+    homogenious_matrix.col(3).head(3) = translation();
+    return homogenious_matrix;
+  }
+
+  /**
+   * \returns 3x4 matrix representation of instance
+   *
+   * It returns the three first row of matrix().
+   */
+  inline
+  const Matrix<Scalar,3,4> matrix3x4() const {
+    Matrix<Scalar,3,4> matrix;
+    matrix.block(0,0,3,3) = rxso3().matrix();
+    matrix.col(3) = translation();
+    return matrix;
+  }
+
+  /**
+   * \brief Assignment operator
+   */
+  template<typename OtherDerived> inline
+  Sim3GroupBase<Derived>& operator=
+  (const Sim3GroupBase<OtherDerived> & other) {
+    rxso3() = other.rxso3();
+    translation() = other.translation();
+    return *this;
+  }
+
+  /**
+   * \brief Group multiplication
+   * \see operator*=()
+   */
+  inline
+  const Sim3Group<Scalar> operator*(const Sim3Group<Scalar>& other) const {
+    Sim3Group<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, scaled and translated version of \f$p\f$
+   *
+   * This function scales, rotates and translates point \f$ p \f$
+   * in \f$ \mathbf{R}^3 \f$ by the Sim3 transformation \f$sR,t\f$
+   * (=scaled rotation matrix, translation vector): \f$ p' = sR\cdot p + t \f$.
+   */
+  inline
+  const Point operator*(const Point & p) const {
+    return rxso3()*p + translation();
+  }
+
+  /**
+   * \brief In-place group multiplication
+   *
+   * \see operator*()
+   */
+  inline
+  void operator*=(const Sim3Group<Scalar>& other) {
+    translation() += (rxso3() * other.translation());
+    rxso3() *= other.rxso3();
+  }
+
+  /**
+   * \brief Mutator of quaternion
+   */
+  inline
+  typename internal::traits<Derived>::RxSO3Type::QuaternionReference
+  quaternion() {
+    return rxso3().quaternion();
+  }
+
+  /**
+   * \brief Accessor of quaternion
+   */
+  inline
+  typename internal::traits<Derived>::RxSO3Type::ConstQuaternionReference
+  quaternion() const {
+    return rxso3().quaternion();
+  }
+
+  /**
+   * \returns Rotation matrix
+   *
+   * deprecated: use rotationMatrix() instead.
+   */
+  inline
+  EIGEN_DEPRECATED const Transformation rotation_matrix() const {
+    return rxso3().rotationMatrix();
+  }
+
+  /**
+   * \returns Rotation matrix
+   */
+  inline
+  const Matrix<Scalar,3,3> rotationMatrix() const {
+    return rxso3().rotationMatrix();
+  }
+
+  /**
+   * \brief Mutator of RxSO3 group
+   */
+  EIGEN_STRONG_INLINE
+  RxSO3Reference rxso3() {
+    return static_cast<Derived*>(this)->rxso3();
+  }
+
+  /**
+   * \brief Accessor of RxSO3 group
+   */
+  EIGEN_STRONG_INLINE
+  ConstRxSO3Reference rxso3() const {
+    return static_cast<const Derived*>(this)->rxso3();
+  }
+
+  /**
+   * \returns scale
+   */
+  EIGEN_STRONG_INLINE
+  const Scalar scale() const {
+    return rxso3().scale();
+  }
+
+  /**
+   * \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 Matrix<Scalar,3,3> & R) {
+    rxso3().setRotationMatrix(R);
+  }
+
+  /**
+   * \brief Scale setter
+   */
+  EIGEN_STRONG_INLINE
+  void setScale(const Scalar & scale) {
+    rxso3().setScale(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 Matrix<Scalar,3,3> & sR) {
+    rxso3().setScaledRotationMatrix(sR);
+  }
+
+  /**
+   * \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();
+  }
+
+  ////////////////////////////////////////////////////////////////////////////
+  // public static functions
+  ////////////////////////////////////////////////////////////////////////////
+
+  /**
+   * \param   b 7-vector representation of Lie algebra element
+   * \returns   derivative of Lie bracket
+   *
+   * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{sim3} \f$
+   * with \f$ [a, b]_{sim3} \f$ being the lieBracket() of the Lie algebra sim3.
+   *
+   * \see lieBracket()
+   */
+  inline static
+  const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
+    const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
+    const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
+    Scalar sigma2 = b[6];
+
+    Adjoint res;
+    res.setZero();
+    res.template topLeftCorner<3,3>()
+        = -SO3::hat(omega2)-sigma2*Matrix3d::Identity();
+    res.template block<3,3>(0,3) = -SO3::hat(upsilon2);
+    res.template topRightCorner<3,1>() = upsilon2;
+    res.template block<3,3>(3,3) = -SO3::hat(omega2);
+    return res;
+  }
+
+  /**
+   * \brief Group exponential
+   *
+   * \param a tangent space element (7-vector)
+   * \returns corresponding element of the group Sim3
+   *
+   * The first three components of \f$ a \f$ represent the translational
+   * part \f$ \upsilon \f$ in the tangent space of Sim3, while the last three
+   * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$.
+   *
+   * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
+   * with \f$ \exp(\cdot) \f$ being the matrix exponential
+   * and \f$ \widehat{\cdot} \f$ the hat()-operator of Sim3.
+   *
+   * \see hat()
+   * \see log()
+   */
+  inline static
+  const Sim3Group<Scalar> exp(const Tangent & a) {
+    const Matrix<Scalar,3,1> & upsilon = a.segment(0,3);
+    const Matrix<Scalar,3,1> & omega = a.segment(3,3);
+    Scalar sigma = a[6];
+    Scalar theta;
+    RxSO3Group<Scalar> rxso3
+        = RxSO3Group<Scalar>::expAndTheta(a.template tail<4>(), &theta);
+    const Matrix<Scalar,3,3> & Omega = SO3Group<Scalar>::hat(omega);
+    const Matrix<Scalar,3,3> & W = calcW(theta, sigma, rxso3.scale(), Omega);
+    return Sim3Group<Scalar>(rxso3, W*upsilon);
+  }
+
+  /**
+   * \brief Generators
+   *
+   * \pre \f$ i \in \{0,1,2,3,4,5,6\} \f$
+   * \returns \f$ i \f$th generator \f$ G_i \f$ of Sim3
+   *
+   * The infinitesimal generators of Sim3 are: \f[
+   *        G_0 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  1\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_1 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  1\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_2 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  1\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right).
+   *        G_3 = \left( \begin{array}{cccc}
+   *                          0&  0&  0&  0\\
+   *                          0&  0& -1&  0\\
+   *                          0&  1&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_4 = \left( \begin{array}{cccc}
+   *                          0&  0&  1&  0\\
+   *                          0&  0&  0&  0\\
+   *                         -1&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_5 = \left( \begin{array}{cccc}
+   *                          0& -1&  0&  0\\
+   *                          1&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right),
+   *        G_6 = \left( \begin{array}{cccc}
+   *                          1&  0&  0&  0\\
+   *                          0&  1&  0&  0\\
+   *                          0&  0&  1&  0\\
+   *                          0&  0&  0&  0\\
+   *                     \end{array} \right).
+   * \f]
+   * \see hat()
+   */
+  inline static
+  const Transformation generator(int i) {
+    if (i<0 || i>6) {
+      throw SophusException("i is not in range [0,6].");
+    }
+    Tangent e;
+    e.setZero();
+    e[i] = static_cast<Scalar>(1);
+    return hat(e);
+  }
+
+  /**
+   * \brief hat-operator
+   *
+   * \param omega 7-vector representation of Lie algebra element
+   * \returns     4x4-matrix representatin of Lie algebra element
+   *
+   * Formally, the hat-operator of Sim3 is defined
+   * as \f$ \widehat{\cdot}: \mathbf{R}^7 \rightarrow \mathbf{R}^{4\times 4},
+   * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$
+   * with \f$ G_i \f$ being the ith infinitesial generator().
+   *
+   * \see generator()
+   * \see vee()
+   */
+  inline static
+  const Transformation hat(const Tangent & v) {
+    Transformation Omega;
+    Omega.template topLeftCorner<3,3>()
+        = RxSO3Group<Scalar>::hat(v.template tail<4>());
+    Omega.col(3).template head<3>() = v.template head<3>();
+    Omega.row(3).setZero();
+    return Omega;
+  }
+
+  /**
+   * \brief Lie bracket
+   *
+   * \param a 7-vector representation of Lie algebra element
+   * \param b 7-vector representation of Lie algebra element
+   * \returns 7-vector representation of Lie algebra element
+   *
+   * It computes the bracket of Sim3. To be more specific, it
+   * computes \f$ [a, b]_{sim3}
+   * := [\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 Sim3.
+   *
+   * \see hat()
+   * \see vee()
+   */
+  inline static
+  const Tangent lieBracket(const Tangent & a,
+                           const Tangent & b) {
+    const Matrix<Scalar,3,1> & upsilon1 = a.template head<3>();
+    const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
+    const Matrix<Scalar,3,1> & omega1 = a.template segment<3>(3);
+    const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
+    Scalar sigma1 = a[6];
+    Scalar sigma2 = b[6];
+
+    Tangent res;
+    res.template head<3>() =
+        SO3Group<Scalar>::hat(omega1)*upsilon2
+        + SO3Group<Scalar>::hat(upsilon1)*omega2
+        + sigma1*upsilon2 - sigma2*upsilon1;
+    res.template segment<3>(3) = omega1.cross(omega2);
+    res[6] = static_cast<Scalar>(0);
+
+    return res;
+  }
+
+  /**
+   * \brief Logarithmic map
+   *
+   * \param other element of the group Sim3
+   * \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 Sim3.
+   *
+   * \see exp()
+   * \see vee()
+   */
+  inline static
+  const Tangent log(const Sim3Group<Scalar> & other) {
+    Tangent res;
+    Scalar theta;
+    const Matrix<Scalar,4,1> & omega_sigma
+        = RxSO3Group<Scalar>::logAndTheta(other.rxso3(), &theta);
+    const Matrix<Scalar,3,1> & omega = omega_sigma.template head<3>();
+    Scalar sigma = omega_sigma[3];
+    const Matrix<Scalar,3,3> & W
+        = calcW(theta, sigma, other.scale(), SO3Group<Scalar>::hat(omega));
+    res.segment(0,3) = W.partialPivLu().solve(other.translation());
+    res.segment(3,3) = omega;
+    res[6] = sigma;
+    return res;
+  }
+
+  /**
+   * \brief vee-operator
+   *
+   * \param Omega 4x4-matrix representation of Lie algebra element
+   * \returns     7-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_sigma;
+    upsilon_omega_sigma.template head<3>()
+        = Omega.col(3).template head<3>();
+    upsilon_omega_sigma.template tail<4>()
+        = RxSO3Group<Scalar>::vee(Omega.template topLeftCorner<3,3>());
+    return upsilon_omega_sigma;
+  }
+
+private:
+  static
+  Matrix<Scalar,3,3> calcW(const Scalar & theta,
+                           const Scalar & sigma,
+                           const Scalar & scale,
+                           const Matrix<Scalar,3,3> & Omega){
+    static const Matrix<Scalar,3,3> I
+        = Matrix<Scalar,3,3>::Identity();
+    static const Scalar one = static_cast<Scalar>(1.);
+    static const Scalar half = static_cast<Scalar>(1./2.);
+    Matrix<Scalar,3,3> Omega2 = Omega*Omega;
+
+    Scalar A,B,C;
+    if (std::abs(sigma)<SophusConstants<Scalar>::epsilon()) {
+      C = one;
+      if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
+        A = half;
+        B = static_cast<Scalar>(1./6.);
+      } else {
+        Scalar theta_sq = theta*theta;
+        A = (one-std::cos(theta))/theta_sq;
+        B = (theta-std::sin(theta))/(theta_sq*theta);
+      }
+    } else {
+      C = (scale-one)/sigma;
+      if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
+        Scalar sigma_sq = sigma*sigma;
+        A = ((sigma-one)*scale+one)/sigma_sq;
+        B = ((half*sigma*sigma-sigma+one)*scale)/(sigma_sq*sigma);
+      } else {
+        Scalar theta_sq = theta*theta;
+        Scalar a = scale*std::sin(theta);
+        Scalar b = scale*std::cos(theta);
+        Scalar c = theta_sq+sigma*sigma;
+        A = (a*sigma+ (one-b)*theta)/(theta*c);
+        B = (C-((b-one)*sigma+a*theta)/(c))*one/(theta_sq);
+      }
+    }
+    return A*Omega + B*Omega2 + C*I;
+  }
+};
+
+/**
+ * \brief Sim3 default type - Constructors and default storage for Sim3 Type
+ */
+template<typename _Scalar, int _Options>
+class Sim3Group : public Sim3GroupBase<Sim3Group<_Scalar,_Options> > {
+  typedef Sim3GroupBase<Sim3Group<_Scalar,_Options> > Base;
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
+  ::Scalar Scalar;
+  /** \brief RxSO3 reference type */
+  typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
+  ::RxSO3Type & RxSO3Reference;
+  /** \brief RxSO3 const reference type */
+  typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
+  ::RxSO3Type & ConstRxSO3Reference;
+  /** \brief translation reference type */
+  typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
+  ::TranslationType & TranslationReference;
+  /** \brief translation const reference type */
+  typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
+  ::TranslationType & ConstTranslationReference;
+
+  /** \brief degree of freedom of group */
+  static const int DoF = Base::DoF;
+  /** \brief number of internal parameters used */
+  static const int num_parameters = Base::num_parameters;
+  /** \brief group transformations are NxN matrices */
+  static const int N = Base::N;
+  /** \brief group transfomation type */
+  typedef typename Base::Transformation Transformation;
+  /** \brief point type */
+  typedef typename Base::Point Point;
+  /** \brief tangent vector type */
+  typedef typename Base::Tangent Tangent;
+  /** \brief adjoint transformation type */
+  typedef typename Base::Adjoint Adjoint;
+
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * \brief Default constructor
+   *
+   * Initialize Quaternion to identity rotation and translation to zero.
+   */
+  inline
+  Sim3Group()
+    : translation_( Matrix<Scalar,3,1>::Zero() )
+  {
+  }
+
+  /**
+   * \brief Copy constructor
+   */
+  template<typename OtherDerived> inline
+  Sim3Group(const Sim3GroupBase<OtherDerived> & other)
+    : rxso3_(other.rxso3()), translation_(other.translation()) {
+  }
+
+  /**
+   * \brief Constructor from RxSO3 and translation vector
+   */
+  template<typename OtherDerived> inline
+  Sim3Group(const RxSO3GroupBase<OtherDerived> & rxso3,
+            const Point & translation)
+    : rxso3_(rxso3), translation_(translation) {
+  }
+
+  /**
+   * \brief Constructor from quaternion and translation vector
+   *
+   * \pre quaternion must not be zero
+   */
+  inline
+  Sim3Group(const Quaternion<Scalar> & quaternion,
+            const Point & translation)
+    : rxso3_(quaternion), translation_(translation) {
+  }
+
+  /**
+   * \brief Constructor from 4x4 matrix
+   *
+   * \pre top-left 3x3 sub-matrix need to be "scaled orthogonal"
+   *      with positive determinant of
+   */
+  inline explicit
+  Sim3Group(const Eigen::Matrix<Scalar,4,4>& T)
+    : rxso3_(T.template topLeftCorner<3,3>()),
+      translation_(T.template block<3,1>(0,3)) {
+  }
+
+  /**
+   * \returns pointer to internal data
+   *
+   * This provides unsafe read/write access to internal data. Sim3 is
+   * represented by a pair of an RxSO3 element (4 parameters) and translation
+   * vector (three parameters).
+   *
+   * Note: The first three Scalars represent the imaginary parts, while the
+   */
+  EIGEN_STRONG_INLINE
+  Scalar* data() {
+    // rxso3_ and translation_ are layed out sequentially with no padding
+    return rxso3_.data();
+  }
+
+  /**
+   * \returns const pointer to internal data
+   *
+   * Const version of data().
+   */
+  EIGEN_STRONG_INLINE
+  const Scalar* data() const {
+    // rxso3_ and translation_ are layed out sequentially with no padding
+    return rxso3_.data();
+  }
+
+  /**
+   * \brief Accessor of RxSO3
+   */
+  EIGEN_STRONG_INLINE
+  RxSO3Reference rxso3() {
+    return rxso3_;
+  }
+
+  /**
+   * \brief Mutator of RxSO3
+   */
+  EIGEN_STRONG_INLINE
+  ConstRxSO3Reference rxso3() const {
+    return rxso3_;
+  }
+
+  /**
+   * \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::RxSO3Group<Scalar> rxso3_;
+  Matrix<Scalar,3,1> translation_;
+};
+
+
+} // end namespace
+
+
+namespace Eigen {
+/**
+ * \brief Specialisation of Eigen::Map for Sim3GroupBase
+ *
+ * Allows us to wrap Sim3 Objects around POD array
+ * (e.g. external c style quaternion)
+ */
+template<typename _Scalar, int _Options>
+class Map<Sophus::Sim3Group<_Scalar>, _Options>
+    : public Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> > {
+  typedef Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> >
+  Base;
+
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Map>::Scalar Scalar;
+  /** \brief translation reference type */
+  typedef typename internal::traits<Map>::TranslationType &
+  TranslationReference;
+  /** \brief translation const reference type */
+  typedef const typename internal::traits<Map>::TranslationType &
+  ConstTranslationReference;
+  /** \brief RxSO3 reference type */
+  typedef typename internal::traits<Map>::RxSO3Type &
+  RxSO3Reference;
+  /** \brief RxSO3 const reference type */
+  typedef const typename internal::traits<Map>::RxSO3Type &
+  ConstRxSO3Reference;
+
+
+  /** \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)
+    : rxso3_(coeffs),
+      translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
+  }
+
+  /**
+   * \brief Mutator of RxSO3
+   */
+  EIGEN_STRONG_INLINE
+  RxSO3Reference rxso3() {
+    return rxso3_;
+  }
+
+  /**
+   * \brief Accessor of RxSO3
+   */
+  EIGEN_STRONG_INLINE
+  ConstRxSO3Reference rxso3() const {
+    return rxso3_;
+  }
+
+  /**
+   * \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::RxSO3Group<Scalar>,_Options> rxso3_;
+  Map<Matrix<Scalar,3,1>,_Options> translation_;
+};
+
+/**
+ * \brief Specialisation of Eigen::Map for const Sim3GroupBase
+ *
+ * Allows us to wrap Sim3 Objects around POD array
+ * (e.g. external c style quaternion)
+ */
+template<typename _Scalar, int _Options>
+class Map<const Sophus::Sim3Group<_Scalar>, _Options>
+    : public Sophus::Sim3GroupBase<
+    Map<const Sophus::Sim3Group<_Scalar>, _Options> > {
+  typedef Sophus::Sim3GroupBase<
+  Map<const Sophus::Sim3Group<_Scalar>, _Options> > Base;
+
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Map>::Scalar Scalar;
+  /** \brief translation type */
+  typedef const typename internal::traits<Map>::TranslationType &
+  ConstTranslationReference;
+  /** \brief RxSO3 const reference type */
+  typedef const typename internal::traits<Map>::RxSO3Type &
+  ConstRxSO3Reference;
+
+  /** \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)
+    : rxso3_(coeffs),
+      translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
+  }
+
+  EIGEN_STRONG_INLINE
+  Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs)
+    : translation_(trans_coeffs), rxso3_(rot_coeffs){
+  }
+
+  /**
+   * \brief Accessor of RxSO3
+   */
+  EIGEN_STRONG_INLINE
+  ConstRxSO3Reference rxso3() const {
+    return rxso3_;
+  }
+
+  /**
+   * \brief Accessor of translation vector
+   */
+  EIGEN_STRONG_INLINE
+  ConstTranslationReference translation() const {
+    return translation_;
+  }
+
+protected:
+  const Map<const Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
+  const Map<const Matrix<Scalar,3,1>,_Options> translation_;
+};
+
+}
+
+#endif

thirdparty/Sophus/sophus/so2.hpp

@@ -0,0 +1,701 @@
+// 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_SO2_HPP
+#define SOPHUS_SO2_HPP
+
+#include <complex>
+
+#include "sophus.hpp"
+
+////////////////////////////////////////////////////////////////////////////
+// Forward Declarations / typedefs
+////////////////////////////////////////////////////////////////////////////
+
+namespace Sophus {
+template<typename _Scalar, int _Options=0> class SO2Group;
+typedef SO2Group<double> SO2 EIGEN_DEPRECATED;
+typedef SO2Group<double> SO2d; /**< double precision SO2 */
+typedef SO2Group<float> SO2f;  /**< single precision SO2 */
+}
+
+////////////////////////////////////////////////////////////////////////////
+// Eigen Traits (For querying derived types in CRTP hierarchy)
+////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////
+// Eigen Traits (For querying derived types in CRTP hierarchy)
+////////////////////////////////////////////////////////////////////////////
+
+namespace Eigen {
+namespace internal {
+
+template<typename _Scalar, int _Options>
+struct traits<Sophus::SO2Group<_Scalar,_Options> > {
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,2,1> ComplexType;
+};
+
+template<typename _Scalar, int _Options>
+struct traits<Map<Sophus::SO2Group<_Scalar>, _Options> >
+    : traits<Sophus::SO2Group<_Scalar, _Options> > {
+  typedef _Scalar Scalar;
+  typedef Map<Matrix<Scalar,2,1>,_Options> ComplexType;
+};
+
+template<typename _Scalar, int _Options>
+struct traits<Map<const Sophus::SO2Group<_Scalar>, _Options> >
+    : traits<const Sophus::SO2Group<_Scalar, _Options> > {
+  typedef _Scalar Scalar;
+  typedef Map<const Matrix<Scalar,2,1>,_Options> ComplexType;
+};
+
+}
+}
+
+namespace Sophus {
+using namespace Eigen;
+
+/**
+ * \brief SO2 base type - implements SO2 class but is storage agnostic
+ *
+ * [add more detailed description/tutorial]
+ */
+template<typename Derived>
+class SO2GroupBase {
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Derived>::Scalar Scalar;
+  /** \brief complex number reference type */
+  typedef typename internal::traits<Derived>::ComplexType &
+  ComplexReference;
+  /** \brief complex number const reference type */
+  typedef const typename internal::traits<Derived>::ComplexType &
+  ConstComplexReference;
+
+  /** \brief degree of freedom of group
+   *         (one for in-plane rotation) */
+  static const int DoF = 1;
+  /** \brief number of internal parameters used
+   *         (unit complex number for rotation) */
+  static const int num_parameters = 2;
+  /** \brief group transformations are NxN matrices */
+  static const int N = 2;
+  /** \brief group transfomation type */
+  typedef Matrix<Scalar,N,N> Transformation;
+  /** \brief point type */
+  typedef Matrix<Scalar,2,1> Point;
+  /** \brief tangent vector type */
+  typedef Scalar Tangent;
+  /** \brief adjoint transformation type */
+  typedef Scalar 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 SO2, it simply returns 1.
+   */
+  inline
+  const Adjoint Adj() const {
+    return 1;
+  }
+
+  /**
+   * \returns copy of instance casted to NewScalarType
+   */
+  template<typename NewScalarType>
+  inline SO2Group<NewScalarType> cast() const {
+    return SO2Group<NewScalarType>(unit_complex()
+                                   .template cast<NewScalarType>() );
+  }
+
+  /**
+   * \returns pointer to internal data
+   *
+   * This provides unsafe read/write access to internal data. SO2 is represented
+   * by a complex number with unit length (two parameters). When using direct
+   * write access, the user needs to take care of that the complex number stays
+   * normalized.
+   *
+   * \see normalize()
+   */
+  inline Scalar* data() {
+    return unit_complex_nonconst().data();
+  }
+
+  /**
+   * \returns const pointer to internal data
+   *
+   * Const version of data().
+   */
+  inline const Scalar* data() const {
+    return unit_complex().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 SO2Group<Scalar>& other) {
+    Scalar lhs_real = unit_complex().x();
+    Scalar lhs_imag = unit_complex().y();
+    const Scalar & rhs_real = other.unit_complex().x();
+    const Scalar & rhs_imag = other.unit_complex().y();
+    // complex multiplication
+    unit_complex_nonconst().x() = lhs_real*rhs_real - lhs_imag*rhs_imag;
+    unit_complex_nonconst().y() = lhs_real*rhs_imag + lhs_imag*rhs_real;
+  }
+
+  /**
+   * \returns group inverse of instance
+   */
+  inline
+  const SO2Group<Scalar> inverse() const {
+    return SO2Group<Scalar>(unit_complex().x(), -unit_complex().y());
+  }
+
+  /**
+   * \brief Logarithmic map
+   *
+   * \returns tangent space representation (=rotation angle) of instance
+   *
+   * \see  log().
+   */
+  inline
+  const Scalar log() const {
+    return SO2Group<Scalar>::log(*this);
+  }
+
+  /**
+   * \brief Normalize complex number
+   *
+   * It re-normalizes complex number to unit length. This method only needs to
+   * be called in conjunction with fastMultiply() or data() write access.
+   */
+  inline
+  void normalize() {
+    Scalar length =
+        std::sqrt(unit_complex().x()*unit_complex().x()
+             + unit_complex().y()*unit_complex().y());
+    if(length < SophusConstants<Scalar>::epsilon()) {
+      throw SophusException("Complex number is (near) zero!");
+    }
+    unit_complex_nonconst().x() /= length;
+    unit_complex_nonconst().y() /= length;
+  }
+
+  /**
+   * \returns 2x2 matrix representation of instance
+   *
+   * For SO2, the matrix representation is an orthogonal matrix R with det(R)=1,
+   * thus the so-called rotation matrix.
+   */
+  inline
+  const Transformation matrix() const {
+    const Scalar & real = unit_complex().x();
+    const Scalar & imag = unit_complex().y();
+    Transformation R;
+    R << real, -imag
+        ,imag,  real;
+    return R;
+  }
+
+  /**
+   * \brief Assignment operator
+   */
+  template<typename OtherDerived> inline
+  SO2GroupBase<Derived>& operator=(const SO2GroupBase<OtherDerived> & other) {
+    unit_complex_nonconst() = other.unit_complex();
+    return *this;
+  }
+
+  /**
+   * \brief Group multiplication
+   * \see operator*=()
+   */
+  inline
+  const SO2Group<Scalar> operator*(const SO2Group<Scalar>& other) const {
+    SO2Group<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 version of \f$p\f$
+   *
+   * This function rotates a point \f$ p \f$ in  \f$ \mathbf{R}^2 \f$ by the
+   * SO2 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$.
+   */
+  inline
+  const Point operator*(const Point & p) const {
+    const Scalar & real = unit_complex().x();
+    const Scalar & imag = unit_complex().y();
+    return Point(real*p[0] - imag*p[1], imag*p[0] + real*p[1]);
+  }
+
+  /**
+   * \brief In-place group multiplication
+   *
+   * \see fastMultiply()
+   * \see operator*()
+   */
+  inline
+  void operator*=(const SO2Group<Scalar>& other) {
+    fastMultiply(other);
+    normalize();
+  }
+
+  /**
+   * \brief Setter of internal unit complex number representation
+   *
+   * \param complex
+   * \pre   the complex number must not be near zero
+   *
+   * The complex number is normalized to unit length.
+   */
+  inline
+  void setComplex(const Point & complex) {
+    unit_complex() = complex;
+    normalize();
+  }
+
+  /**
+   * \brief Accessor of unit complex number
+   *
+   * No direct write access is given to ensure the complex stays normalized.
+   */
+  EIGEN_STRONG_INLINE
+  ConstComplexReference unit_complex() const {
+    return static_cast<const Derived*>(this)->unit_complex();
+  }
+
+  ////////////////////////////////////////////////////////////////////////////
+  // public static functions
+  ////////////////////////////////////////////////////////////////////////////
+
+  /**
+   * \brief Group exponential
+   *
+   * \param theta tangent space element (=rotation angle \f$ \theta \f$)
+   * \returns     corresponding element of the group SO2
+   *
+   * To be more specific, this function computes \f$ \exp(\widehat{\theta}) \f$
+   * with \f$ \exp(\cdot) \f$ being the matrix exponential
+   * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO2.
+   *
+   * \see hat()
+   * \see log()
+   */
+  inline static
+  const SO2Group<Scalar> exp(const Tangent & theta) {
+    return SO2Group<Scalar>(std::cos(theta), std::sin(theta));
+  }
+
+  /**
+   * \brief Generator
+   *
+   * The infinitesimal generator of SO2
+   * is \f$
+   *        G_0 = \left( \begin{array}{ccc}
+   *                          0& -1& \\
+   *                          1&  0&
+   *                     \end{array} \right).
+   * \f$
+   * \see hat()
+   */
+  inline static
+  const Transformation generator() {
+    return hat(1);
+  }
+
+  /**
+   * \brief hat-operator
+   *
+   * \param theta scalar representation of Lie algebra element
+   * \returns     2x2-matrix representatin of Lie algebra element
+   *
+   * Formally, the hat-operator of SO2 is defined
+   * as \f$ \widehat{\cdot}: \mathbf{R}^2 \rightarrow \mathbf{R}^{2\times 2},
+   * \quad \widehat{\theta} = G_0\cdot \theta \f$
+   * with \f$ G_0 \f$ being the infinitesial generator().
+   *
+   * \see generator()
+   * \see vee()
+   */
+  inline static
+  const Transformation hat(const Tangent & theta) {
+    Transformation Omega;
+    Omega <<  static_cast<Scalar>(0), -theta
+        ,  theta,     static_cast<Scalar>(0);
+    return Omega;
+  }
+
+  /**
+   * \brief Lie bracket
+   *
+   * \param theta1 scalar representation of Lie algebra element
+   * \param theta2 scalar representation of Lie algebra element
+   * \returns      zero
+   *
+   * It computes the bracket. For the Lie algebra so2, the Lie bracket is
+   * simply \f$ [\theta_1, \theta_2]_{so2} = 0 \f$ since SO2 is a
+   * commutative group.
+   *
+   * \see hat()
+   * \see vee()
+   */
+  inline static
+  const Tangent lieBracket(const Tangent & theta1,
+                           const Tangent & theta2) {
+    return static_cast<Scalar>(0);
+  }
+
+  /**
+   * \brief Logarithmic map
+   *
+   * \param other element of the group SO2
+   * \returns     corresponding tangent space element
+   *              (=rotation angle \f$ \theta \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 SO2.
+   *
+   * \see exp()
+   * \see vee()
+   */
+  inline static
+  const Tangent log(const SO2Group<Scalar> & other) {
+    // todo: general implementation for Scalar not being float or double.
+    return atan2(other.unit_complex_.y(), other.unit_complex().x());
+  }
+
+  /**
+   * \brief vee-operator
+   *
+   * \param Omega 2x2-matrix representation of Lie algebra element
+   * \pre         Omega need to be a skew-symmetric matrix
+   * \returns     scalar representatin of Lie algebra element
+   *s
+   * This is the inverse of the hat()-operator.
+   *
+   * \see hat()
+   */
+  inline static
+  const Tangent vee(const Transformation & Omega) {
+    return static_cast<Scalar>(0.5)*(Omega(1,0) - Omega(0,1));
+  }
+
+private:
+  // Mutator of complex number is private so users are hampered
+  // from setting non-unit complex numbers.
+  EIGEN_STRONG_INLINE
+  ComplexReference unit_complex_nonconst() {
+    return static_cast<Derived*>(this)->unit_complex_nonconst();
+  }
+
+};
+
+/**
+ * \brief SO2 default type - Constructors and default storage for SO2 Type
+ */
+template<typename _Scalar, int _Options>
+class SO2Group : public SO2GroupBase<SO2Group<_Scalar,_Options> > {
+  typedef SO2GroupBase<SO2Group<_Scalar,_Options> > Base;
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<SO2Group<_Scalar,_Options> >
+  ::Scalar Scalar;
+  /** \brief complex number reference type */
+  typedef typename internal::traits<SO2Group<_Scalar,_Options> >
+  ::ComplexType & ComplexReference;
+  /** \brief complex number const reference type */
+  typedef const typename internal::traits<SO2Group<_Scalar,_Options> >
+  ::ComplexType & ConstComplexReference;
+
+  /** \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_complex_nonconst can be accessed from base
+  friend class SO2GroupBase<SO2Group<_Scalar,_Options> >;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * \brief Default constructor
+   *
+   * Initialize complex number to identity rotation.
+   */
+  inline SO2Group()
+    : unit_complex_(static_cast<Scalar>(1), static_cast<Scalar>(0)) {
+  }
+
+  /**
+   * \brief Copy constructor
+   */
+  template<typename OtherDerived> inline
+  SO2Group(const SO2GroupBase<OtherDerived> & other)
+    : unit_complex_(other.unit_complex()) {
+  }
+
+  /**
+   * \brief Constructor from rotation matrix
+   *
+   * \pre rotation matrix need to be orthogonal with determinant of 1
+   */
+  inline explicit
+  SO2Group(const Transformation & R)
+    : unit_complex_(static_cast<Scalar>(0.5)*(R(0,0)+R(1,1)),
+                    static_cast<Scalar>(0.5)*(R(1,0)-R(0,1))) {
+    if (std::abs(R.determinant()-static_cast<Scalar>(1))
+        > SophusConstants<Scalar>::epsilon()) {
+      throw SophusException("det(R) is not near 1.");
+    }
+  }
+
+  /**
+   * \brief Constructor from pair of real and imaginary number
+   *
+   * \pre pair must not be zero
+   */
+  inline SO2Group(const Scalar & real, const Scalar & imag)
+    : unit_complex_(real, imag) {
+    Base::normalize();
+  }
+
+  /**
+   * \brief Constructor from 2-vector
+   *
+   * \pre vector must not be zero
+   */
+  inline explicit
+  SO2Group(const Matrix<Scalar,2,1> & complex)
+    : unit_complex_(complex) {
+    Base::normalize();
+  }
+
+  /**
+   * \brief Constructor from std::complex
+   *
+   * \pre complex number must not be zero
+   */
+  inline explicit
+  SO2Group(const std::complex<Scalar> & complex)
+    : unit_complex_(complex.real(), complex.imag()) {
+    Base::normalize();
+  }
+
+  /**
+   * \brief Constructor from an angle
+   */
+  inline explicit
+  SO2Group(Scalar theta) {
+    unit_complex_nonconst() = SO2Group<Scalar>::exp(theta).unit_complex();
+  }
+
+  /**
+   * \brief Accessor of unit complex number
+   *
+   * No direct write access is given to ensure the complex number stays
+   * normalized.
+   */
+  EIGEN_STRONG_INLINE
+  ConstComplexReference unit_complex() const {
+    return unit_complex_;
+  }
+
+protected:
+  // Mutator of complex number is protected so users are hampered
+  // from setting non-unit complex numbers.
+  EIGEN_STRONG_INLINE
+  ComplexReference unit_complex_nonconst() {
+    return unit_complex_;
+  }
+
+  static bool isNearZero(const Scalar & real, const Scalar & imag) {
+    return (real*real + imag*imag < SophusConstants<Scalar>::epsilon());
+  }
+
+  Matrix<Scalar,2,1> unit_complex_;
+};
+
+} // end namespace
+
+
+namespace Eigen {
+/**
+ * \brief Specialisation of Eigen::Map for SO2GroupBase
+ *
+ * Allows us to wrap SO2 Objects around POD array
+ * (e.g. external c style complex number)
+ */
+template<typename _Scalar, int _Options>
+class Map<Sophus::SO2Group<_Scalar>, _Options>
+    : public Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> > {
+  typedef Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> > Base;
+
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Map>::Scalar Scalar;
+  /** \brief complex number reference type */
+  typedef typename internal::traits<Map>::ComplexType & ComplexReference;
+  /** \brief complex number const reference type */
+  typedef const typename internal::traits<Map>::ComplexType &
+  ConstComplexReference;
+
+  /** \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_complex_nonconst can be accessed from base
+  friend class Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> >;
+
+  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
+  using Base::operator*=;
+  using Base::operator*;
+
+  EIGEN_STRONG_INLINE
+  Map(Scalar* coeffs) : unit_complex_(coeffs) {
+  }
+
+  /**
+   * \brief Accessor of unit complex number
+   *
+   * No direct write access is given to ensure the complex number stays
+   * normalized.
+   */
+  EIGEN_STRONG_INLINE
+  ConstComplexReference unit_complex() const {
+    return unit_complex_;
+  }
+
+protected:
+  // Mutator of complex number is protected so users are hampered
+  // from setting non-unit complex number.
+  EIGEN_STRONG_INLINE
+  ComplexReference unit_complex_nonconst() {
+    return unit_complex_;
+  }
+
+  Map<Matrix<Scalar,2,1>,_Options> unit_complex_;
+};
+
+/**
+ * \brief Specialisation of Eigen::Map for const SO2GroupBase
+ *
+ * Allows us to wrap SO2 Objects around POD array
+ * (e.g. external c style complex number)
+ */
+template<typename _Scalar, int _Options>
+class Map<const Sophus::SO2Group<_Scalar>, _Options>
+    : public Sophus::SO2GroupBase<
+    Map<const Sophus::SO2Group<_Scalar>, _Options> > {
+  typedef Sophus::SO2GroupBase<Map<const Sophus::SO2Group<_Scalar>, _Options> >
+  Base;
+
+public:
+  /** \brief scalar type */
+  typedef typename internal::traits<Map>::Scalar Scalar;
+  /** \brief complex number const reference type */
+  typedef const typename internal::traits<Map>::ComplexType &
+  ConstComplexReference;
+
+
+  /** \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_complex_(coeffs) {
+  }
+
+  /**
+   * \brief Accessor of unit complex number
+   *
+   * No direct write access is given to ensure the complex number stays
+   * normalized.
+   */
+  EIGEN_STRONG_INLINE
+  ConstComplexReference unit_complex() const {
+    return unit_complex_;
+  }
+
+protected:
+  const Map<const Matrix<Scalar,2,1>,_Options> unit_complex_;
+};
+
+}
+
+
+#endif // SOPHUS_SO2_HPP

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

thirdparty/Sophus/sophus/sophus.hpp

@@ -0,0 +1,77 @@
+// This file is part of Sophus.
+//
+// Copyright 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_HPP
+#define SOPHUS_HPP
+
+#include <stdexcept>
+
+// fix log1p not being found on Android in Eigen
+#if defined( ANDROID )
+#include <cmath>
+namespace std {
+  using ::log1p;
+}
+#endif
+
+#include <Eigen/Eigen>
+#include <Eigen/Geometry>
+
+namespace Sophus {
+using namespace Eigen;
+
+template<typename Scalar>
+struct SophusConstants {
+  EIGEN_ALWAYS_INLINE static
+  const Scalar epsilon() {
+    return static_cast<Scalar>(1e-10);
+  }
+
+  EIGEN_ALWAYS_INLINE static
+  const Scalar pi() {
+    return static_cast<Scalar>(M_PI);
+  }
+};
+
+template<>
+struct SophusConstants<float> {
+  EIGEN_ALWAYS_INLINE static
+  float epsilon() {
+    return static_cast<float>(1e-5);
+  }
+
+  EIGEN_ALWAYS_INLINE static
+  float pi() {
+    return static_cast<float>(M_PI);
+  }
+};
+
+class SophusException : public std::runtime_error {
+public:
+  SophusException (const std::string& str)
+    : runtime_error("Sophus exception: " + str) {
+  }
+};
+
+}
+
+#endif

thirdparty/Sophus/sophus/test_rxso3.cpp

@@ -0,0 +1,92 @@
+// 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.
+
+#include <iostream>
+#include <vector>
+
+
+#include "rxso3.hpp"
+#include "tests.hpp"
+
+using namespace Sophus;
+using namespace std;
+
+template<class Scalar>
+void tests() {
+
+  typedef RxSO3Group<Scalar> RxSO3Type;
+  typedef typename RxSO3Group<Scalar>::Point Point;
+  typedef typename RxSO3Group<Scalar>::Tangent Tangent;
+
+  vector<RxSO3Type> rxso3_vec;
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0))
+                      *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0))
+                      *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0)));
+  rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0))
+                      *RxSO3Type::exp(Tangent(M_PI, 0, 0,0))
+                      *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0)));
+
+  vector<Tangent> tangent_vec;
+  Tangent tmp;
+  tmp << 0,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 1,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 1,0,0,0.1;
+  tangent_vec.push_back(tmp);
+  tmp << 0,1,0,0.1;
+  tangent_vec.push_back(tmp);
+  tmp << 0,0,1,-0.1;
+  tangent_vec.push_back(tmp);
+  tmp << -1,1,0,-0.1;
+  tangent_vec.push_back(tmp);
+  tmp << 20,-1,0,2;
+  tangent_vec.push_back(tmp);
+
+  vector<Point> point_vec;
+  point_vec.push_back(Point(1,2,4));
+
+  Tests<RxSO3Type> tests;
+  tests.setGroupElements(rxso3_vec);
+  tests.setTangentVectors(tangent_vec);
+  tests.setPoints(point_vec);
+
+  tests.runAllTests();
+}
+
+int main() {
+  cerr << "Test RxSO3" << endl << endl;
+
+  cerr << "Double tests: " << endl;
+  tests<double>();
+
+  cerr << "Float tests: " << endl;
+  tests<float>();
+  return 0;
+}

thirdparty/Sophus/sophus/test_se2.cpp

@@ -0,0 +1,90 @@
+// 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.
+
+#include <iostream>
+#include <vector>
+
+#include <unsupported/Eigen/MatrixFunctions>
+#include "se2.hpp"
+#include "tests.hpp"
+
+using namespace Sophus;
+using namespace std;
+
+template<class Scalar>
+void tests() {
+
+  typedef SO2Group<Scalar> SO2Type;
+  typedef SE2Group<Scalar> SE2Type;
+  typedef typename SE2Group<Scalar>::Point Point;
+  typedef typename SE2Group<Scalar>::Tangent Tangent;
+
+  vector<SE2Type> se2_vec;
+  se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0)));
+  se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0)));
+  se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100)));
+  se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1)));
+  se2_vec.push_back(SE2Type(SO2Type(0.00001),
+                            Point(-0.00000001,0.0000000001)));
+  se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0))
+                    *SE2Type(SO2Type(M_PI),Point(0,0))
+                    *SE2Type(SO2Type(-0.2),Point(0,0)));
+  se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0))
+                    *SE2Type(SO2Type(M_PI),Point(0,0))
+                    *SE2Type(SO2Type(-0.3),Point(0,6)));
+
+  vector<Tangent> tangent_vec;
+  Tangent tmp;
+  tmp << 0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 1,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 0,1,1;
+  tangent_vec.push_back(tmp);
+  tmp << -1,1,0;
+  tangent_vec.push_back(tmp);
+  tmp << 20,-1,-1;
+  tangent_vec.push_back(tmp);
+  tmp << 30,5,20;
+  tangent_vec.push_back(tmp);
+
+  vector<Point> point_vec;
+  point_vec.push_back(Point(1,2));
+
+  Tests<SE2Type> tests;
+  tests.setGroupElements(se2_vec);
+  tests.setTangentVectors(tangent_vec);
+  tests.setPoints(point_vec);
+
+  tests.runAllTests();
+}
+
+int main() {
+  cerr << "Test SE2" << endl << endl;
+
+  cerr << "Double tests: " << endl;
+  tests<double>();
+
+  cerr << "Float tests: " << endl;
+  tests<float>();
+  return 0;
+}

thirdparty/Sophus/sophus/test_se3.cpp

@@ -0,0 +1,105 @@
+// 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.
+
+#include <iostream>
+#include <vector>
+
+#include "se3.hpp"
+#include "tests.hpp"
+
+using namespace Sophus;
+using namespace std;
+
+template<class Scalar>
+void tests() {
+
+  typedef SO3Group<Scalar> SO3Type;
+  typedef SE3Group<Scalar> SE3Type;
+  typedef typename SE3Group<Scalar>::Point Point;
+  typedef typename SE3Group<Scalar>::Tangent Tangent;
+
+  vector<SE3Type> se3_vec;
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)),
+                            Point(0,0,0)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)),
+                            Point(10,0,0)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)),
+                            Point(0,100,5)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
+                            Point(0,0,0)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
+                            Point(0,-0.00000001,0.0000000001)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
+                            Point(0.01,0,0)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(M_PI, 0, 0)),
+                            Point(4,-5,0)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)),
+                            Point(0,0,0))
+                    *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)),
+                             Point(0,0,0))
+                    *SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)),
+                             Point(0,0,0)));
+  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)),
+                            Point(2,0,-7))
+                    *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)),
+                             Point(0,0,0))
+                    *SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)),
+                             Point(0,6,0)));
+  vector<Tangent> tangent_vec;
+  Tangent tmp;
+  tmp << 0,0,0,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 1,0,0,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 0,1,0,1,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 0,-5,10,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << -1,1,0,0,0,1;
+  tangent_vec.push_back(tmp);
+  tmp << 20,-1,0,-1,1,0;
+  tangent_vec.push_back(tmp);
+  tmp << 30,5,-1,20,-1,0;
+  tangent_vec.push_back(tmp);
+
+  vector<Point> point_vec;
+  point_vec.push_back(Point(1,2,4));
+
+  Tests<SE3Type> tests;
+  tests.setGroupElements(se3_vec);
+  tests.setTangentVectors(tangent_vec);
+  tests.setPoints(point_vec);
+
+  tests.runAllTests();
+  cerr << "passed." << endl << endl;
+}
+
+int main() {
+  cerr << "Test SE3" << endl << endl;
+
+  cerr << "Double tests: " << endl;
+  tests<double>();
+
+  cerr << "Float tests: " << endl;
+  tests<float>();
+  return 0;
+}

thirdparty/Sophus/sophus/test_sim3.cpp

@@ -0,0 +1,109 @@
+// 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.
+
+#include <iostream>
+#include <vector>
+
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include "sim3.hpp"
+#include "tests.hpp"
+
+using namespace Sophus;
+using namespace std;
+
+template<class Scalar>
+void tests() {
+
+  typedef Sim3Group<Scalar> Sim3Type;
+  typedef RxSO3Group<Scalar> RxSO3Type;
+  typedef typename Sim3Group<Scalar>::Point Point;
+  typedef typename Sim3Group<Scalar>::Tangent Tangent;
+  typedef Matrix<Scalar,4,1> Vector4Type;
+
+  vector<Sim3Type> sim3_vec;
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,1.)),
+                              Point(0,0,0)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, -1.0,1.1)),
+                              Point(10,0,0)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.,1.1)),
+                              Point(0,10,5)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0.)),
+                              Point(0,0,0)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(
+                                Vector4Type(0., 0., 0.00001, 0.0000001)),
+                              Point(1,-1.00000001,2.0000000001)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0)),
+                              Point(0.01,0,0)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0.9)),
+                              Point(4,-5,0)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,0)),
+                              Point(0,0,0))
+                     *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)),
+                               Point(0,0,0))
+                     *Sim3Type(RxSO3Type::exp(Vector4Type(-0.2, -0.5, -0.0,0)),
+                               Point(0,0,0)));
+  sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.3, 0.5, 0.1,0)),
+                              Point(2,0,-7))
+                     *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)),
+                               Point(0,0,0))
+                     *Sim3Type(RxSO3Type::exp(Vector4Type(-0.3, -0.5, -0.1,0)),
+                               Point(0,6,0)));
+  vector<Tangent> tangent_vec;
+  Tangent tmp;
+  tmp << 0,0,0,0,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 1,0,0,0,0,0,0;
+  tangent_vec.push_back(tmp);
+  tmp << 0,1,0,1,0,0,0.1;
+  tangent_vec.push_back(tmp);
+  tmp << 0,0,1,0,1,0,0.1;
+  tangent_vec.push_back(tmp);
+  tmp << -1,1,0,0,0,1,-0.1;
+  tangent_vec.push_back(tmp);
+  tmp << 20,-1,0,-1,1,0,-0.1;
+  tangent_vec.push_back(tmp);
+  tmp << 30,5,-1,20,-1,0,1.5;
+  tangent_vec.push_back(tmp);
+
+
+  vector<Point> point_vec;
+  point_vec.push_back(Point(1,2,4));
+
+  Tests<Sim3Type> tests;
+  tests.setGroupElements(sim3_vec);
+  tests.setTangentVectors(tangent_vec);
+  tests.setPoints(point_vec);
+
+  tests.runAllTests();
+}
+
+int main() {
+  cerr << "Test Sim3" << endl << endl;
+
+  cerr << "Double tests: " << endl;
+  tests<double>();
+
+  cerr << "Float tests: " << endl;
+  tests<float>();
+  return 0;
+}

thirdparty/Sophus/sophus/test_so2.cpp

@@ -0,0 +1,91 @@
+// 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.
+
+
+#include <iostream>
+#include <vector>
+
+#include "so2.hpp"
+#include "tests.hpp"
+
+using namespace Sophus;
+using namespace std;
+
+template<class Scalar>
+void tests() {
+
+  typedef SO2Group<Scalar> SO2Type;
+  typedef typename SO2Group<Scalar>::Point Point;
+  typedef typename SO2Group<Scalar>::Tangent Tangent;
+
+  vector<SO2Type> so2_vec;
+  so2_vec.push_back(SO2Type::exp(0.0));
+  so2_vec.push_back(SO2Type::exp(0.2));
+  so2_vec.push_back(SO2Type::exp(10.));
+  so2_vec.push_back(SO2Type::exp(0.00001));
+  so2_vec.push_back(SO2Type::exp(M_PI));
+  so2_vec.push_back(SO2Type::exp(0.2)
+                    *SO2Type::exp(M_PI)
+                    *SO2Type::exp(-0.2));
+  so2_vec.push_back(SO2Type::exp(-0.3)
+                    *SO2Type::exp(M_PI)
+                    *SO2Type::exp(0.3));
+
+  vector<Tangent> tangent_vec;
+  tangent_vec.push_back(Tangent(0));
+  tangent_vec.push_back(Tangent(1));
+  tangent_vec.push_back(Tangent(M_PI_2));
+  tangent_vec.push_back(Tangent(-1));
+  tangent_vec.push_back(Tangent(20));
+  tangent_vec.push_back(Tangent(M_PI_2+0.0001));
+
+  vector<Point> point_vec;
+  point_vec.push_back(Point(1,2));
+
+  Tests<SO2Type> tests;
+  tests.setGroupElements(so2_vec);
+  tests.setTangentVectors(tangent_vec);
+  tests.setPoints(point_vec);
+
+  tests.runAllTests();
+
+  cerr << "Exception test: ";
+  try {
+    SO2Type so2(0., 0.);
+  } catch(SophusException & e) {
+    cerr << "passed." << endl << endl;
+    return;
+  }
+  cerr << "failed!" << endl << endl;
+  exit(-1);
+}
+
+int main() {
+  cerr << "Test SO2" << endl << endl;
+
+  cerr << "Double tests: " << endl;
+  tests<double>();
+
+  cerr << "Float tests: " << endl;
+  tests<float>();
+  return 0;
+}

thirdparty/Sophus/sophus/test_so3.cpp

@@ -0,0 +1,84 @@
+// 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.
+
+#include <iostream>
+#include <vector>
+
+#include "so3.hpp"
+#include "tests.hpp"
+
+using namespace Sophus;
+using namespace std;
+
+template<class Scalar>
+void tests() {
+
+  typedef SO3Group<Scalar> SO3Type;
+  typedef typename SO3Group<Scalar>::Point Point;
+  typedef typename SO3Group<Scalar>::Tangent Tangent;
+
+  vector<SO3Type> so3_vec;
+
+  so3_vec.push_back(SO3Type(Quaternion<Scalar>(0.1e-11, 0., 1., 0.)));
+  so3_vec.push_back(SO3Type(Quaternion<Scalar>(-1,0.00001,0.0,0.0)));
+  so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0)));
+  so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, -1.0)));
+  so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.)));
+  so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.00001)));
+  so3_vec.push_back(SO3Type::exp(Point(M_PI, 0, 0)));
+  so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0))
+                    *SO3Type::exp(Point(M_PI, 0, 0))
+                    *SO3Type::exp(Point(-0.2, -0.5, -0.0)));
+  so3_vec.push_back(SO3Type::exp(Point(0.3, 0.5, 0.1))
+                    *SO3Type::exp(Point(M_PI, 0, 0))
+                    *SO3Type::exp(Point(-0.3, -0.5, -0.1)));
+
+  vector<Tangent> tangent_vec;
+  tangent_vec.push_back(Tangent(0,0,0));
+  tangent_vec.push_back(Tangent(1,0,0));
+  tangent_vec.push_back(Tangent(0,1,0));
+  tangent_vec.push_back(Tangent(M_PI_2,M_PI_2,0.0));
+  tangent_vec.push_back(Tangent(-1,1,0));
+  tangent_vec.push_back(Tangent(20,-1,0));
+  tangent_vec.push_back(Tangent(30,5,-1));
+
+  vector<Point> point_vec;
+  point_vec.push_back(Point(1,2,4));
+
+  Tests<SO3Type> tests;
+  tests.setGroupElements(so3_vec);
+  tests.setTangentVectors(tangent_vec);
+  tests.setPoints(point_vec);
+
+  tests.runAllTests();
+}
+
+int main() {
+  cerr << "Test SO3" << endl << endl;
+
+  cerr << "Double tests: " << endl;
+  tests<double>();
+
+  cerr << "Float tests: " << endl;
+  tests<float>();
+  return 0;
+}

thirdparty/Sophus/sophus/tests.hpp

@@ -0,0 +1,264 @@
+#ifndef SOPUHS_TESTS_HPP
+#define SOPUHS_TESTS_HPP
+
+#include <vector>
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include "sophus.hpp"
+
+namespace Sophus {
+
+using namespace std;
+using namespace Eigen;
+
+template <class LieGroup>
+class Tests {
+
+public:
+  typedef typename LieGroup::Scalar Scalar;
+  typedef typename LieGroup::Transformation Transformation;
+  typedef typename LieGroup::Tangent Tangent;
+  typedef typename LieGroup::Point Point;
+  typedef typename LieGroup::Adjoint Adjoint;
+  static const int N = LieGroup::N;
+  static const int DoF = LieGroup::DoF;
+
+  const Scalar SMALL_EPS;
+
+  Tests() : SMALL_EPS(SophusConstants<Scalar>::epsilon()) {
+  }
+
+  void setGroupElements(const vector<LieGroup> & group_vec) {
+    group_vec_  = group_vec;
+  }
+
+  void setTangentVectors(const vector<Tangent> & tangent_vec) {
+    tangent_vec_  = tangent_vec;
+  }
+
+  void setPoints(const vector<Point> & point_vec) {
+    point_vec_  = point_vec;
+  }
+
+  bool adjointTest() {
+    bool passed = true;
+    for (size_t i=0; i<group_vec_.size(); ++i) {
+      Transformation T = group_vec_[i].matrix();
+      Adjoint Ad = group_vec_[i].Adj();
+      for (size_t j=0; j<tangent_vec_.size(); ++j) {
+        Tangent x = tangent_vec_[j];
+
+        Transformation I;
+        I.setIdentity();
+        Tangent ad1 = Ad*x;
+        Tangent ad2 = LieGroup::vee(T*LieGroup::hat(x)
+                                    *group_vec_[i].inverse().matrix());
+        Scalar nrm = norm(ad1-ad2);
+
+        if (isnan(nrm) || nrm>20.*SMALL_EPS) {
+          cerr << "Adjoint" << endl;
+          cerr  << "Test case: " << i << "," << j <<endl;
+          cerr << (ad1-ad2) <<endl;
+          cerr << endl;
+          passed = false;
+        }
+      }
+    }
+    return passed;
+  }
+
+  bool expLogTest() {
+    bool passed = true;
+
+    for (size_t i=0; i<group_vec_.size(); ++i) {
+      Transformation T1 = group_vec_[i].matrix();
+      Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix();
+      Transformation DiffT = T1-T2;
+      Scalar nrm = DiffT.norm();
+
+      if (isnan(nrm) || nrm>SMALL_EPS) {
+        cerr << "G - exp(log(G))" << endl;
+        cerr  << "Test case: " << i << endl;
+        cerr << DiffT <<endl;
+        cerr << endl;
+        passed = false;
+      }
+    }
+    return passed;
+  }
+
+  bool expMapTest() {
+    bool passed = true;
+    for (size_t i=0; i<tangent_vec_.size(); ++i) {
+
+      Tangent omega = tangent_vec_[i];
+      Transformation exp_x = LieGroup::exp(omega).matrix();
+      Transformation expmap_hat_x = (LieGroup::hat(omega)).exp();
+      Transformation DiffR = exp_x-expmap_hat_x;
+      Scalar nrm = DiffR.norm();
+
+      if (isnan(nrm) || nrm>10.*SMALL_EPS) {
+        cerr << "expmap(hat(x)) - exp(x)" << endl;
+        cerr  << "Test case: " << i << endl;
+        cerr << exp_x <<endl;
+        cerr << expmap_hat_x <<endl;
+        cerr << DiffR <<endl;
+        cerr << endl;
+        passed = false;
+      }
+    }
+    return passed;
+  }
+
+  bool groupActionTest() {
+    bool passed = true;
+
+    for (size_t i=0; i<group_vec_.size(); ++i) {
+      for (size_t j=0; j<point_vec_.size(); ++j) {
+        const Point & p = point_vec_[j];
+        Transformation T = group_vec_[i].matrix();
+        Point res1 = group_vec_[i]*p;
+        Point res2 = map(T, p);
+        Scalar nrm = (res1-res2).norm();
+        if (isnan(nrm) || nrm>SMALL_EPS) {
+          cerr << "Transform vector" << endl;
+          cerr  << "Test case: " << i << endl;
+          cerr << (res1-res2) <<endl;
+          cerr << endl;
+          passed = false;
+        }
+      }
+    }
+    return passed;
+  }
+
+
+  bool lieBracketTest() {
+    bool passed = true;
+    for (size_t i=0; i<tangent_vec_.size(); ++i) {
+      for (size_t j=0; j<tangent_vec_.size(); ++j) {
+        Tangent res1 = LieGroup::lieBracket(tangent_vec_[i],tangent_vec_[j]);
+        Transformation hati = LieGroup::hat(tangent_vec_[i]);
+        Transformation hatj = LieGroup::hat(tangent_vec_[j]);
+
+        Tangent res2 = LieGroup::vee(hati*hatj-hatj*hati);
+        Tangent resDiff = res1-res2;
+        if (norm(resDiff)>SMALL_EPS) {
+          cerr << "Lie Bracket Test" << endl;
+          cerr  << "Test case: " << i << ", " <<j<< endl;
+          cerr << resDiff << endl;
+          cerr << endl;
+          passed = false;
+        }
+      }
+    }
+    return passed;
+  }
+
+  bool mapAndMultTest() {
+    bool passed = true;
+    for (size_t i=0; i<group_vec_.size(); ++i) {
+      for (size_t j=0; j<group_vec_.size(); ++j) {
+        Transformation mul_resmat = (group_vec_[i]*group_vec_[j]).matrix();
+        Scalar fastmul_res_raw[LieGroup::num_parameters];
+        Eigen::Map<LieGroup> fastmul_res(fastmul_res_raw);
+        Eigen::Map<const LieGroup> group_j_constmap(group_vec_[j].data());
+        fastmul_res = group_vec_[i];
+        fastmul_res.fastMultiply(group_j_constmap);
+        Transformation diff =  mul_resmat-fastmul_res.matrix();
+        Scalar nrm = diff.norm();
+        if (isnan(nrm) || nrm>SMALL_EPS) {
+          cerr << "Map & Multiply" << endl;
+          cerr  << "Test case: " << i  << "," << j << endl;
+          cerr << diff <<endl;
+          cerr << endl;
+          passed = false;
+        }
+      }
+    }
+    return passed;
+  }
+
+  bool veeHatTest() {
+    bool passed = true;
+    for (size_t i=0; i<tangent_vec_.size(); ++i) {
+      Tangent resDiff
+          = tangent_vec_[i] - LieGroup::vee(LieGroup::hat(tangent_vec_[i]));
+      if (norm(resDiff)>SMALL_EPS) {
+        cerr << "Hat-vee Test" << endl;
+        cerr  << "Test case: " << i <<  endl;
+        cerr << resDiff << endl;
+        cerr << endl;
+        passed = false;
+      }
+    }
+    return passed;
+  }
+
+
+
+  void runAllTests() {
+    bool passed = adjointTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    passed = expLogTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    passed = expMapTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    passed = groupActionTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    passed = lieBracketTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    passed = mapAndMultTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    passed = veeHatTest();
+    if (!passed) {
+      cerr << "failed!" << endl << endl;
+      exit(-1);
+    }
+    cerr << "passed." << endl << endl;
+  }
+
+private:
+  Matrix<Scalar,N-1,1> map(const Matrix<Scalar,N,N> & T,
+                           const Matrix<Scalar,N-1,1> & p) {
+    return T.template topLeftCorner<N-1,N-1>()*p
+        + T.template topRightCorner<N-1,1>();
+  }
+
+  Matrix<Scalar,N,1> map(const Matrix<Scalar,N,N> & T,
+                         const Matrix<Scalar,N,1> & p) {
+    return T*p;
+  }
+
+  Scalar norm(const Scalar & v) {
+    return std::abs(v);
+  }
+
+  Scalar norm(const Matrix<Scalar,DoF,1> & T) {
+    return T.norm();
+  }
+
+  std::vector<LieGroup> group_vec_;
+  std::vector<Tangent> tangent_vec_;
+  std::vector<Point> point_vec_;
+};
+}
+#endif // TESTS_HPP