pi/ar_dso
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Files
master
ar_dso/thirdparty/Sophus/sophus/se2.hpp
Ivan e4c8529305 v1
2022-06-28 10:36:24 +03:00

908 lines
26 KiB
C++
Raw Permalink Blame History

// 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
Reference in New Issue View Git Blame Copy Permalink