v1
This commit is contained in:
Ivan
701
thirdparty/Sophus/sophus/so2.hpp
vendored
Normal file
701
thirdparty/Sophus/sophus/so2.hpp
vendored
Normal file
@@ -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
|
||||
Reference in New Issue
Block a user