702 lines
20 KiB
C++
702 lines
20 KiB
C++
// 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
|