manif-geom-cpp
|
Class representing a member of the \(SO(2)\) manifold, or a 2D rotation. More...
#include <SO2.h>
Public Member Functions | |
SO2 () | |
Create a rotation (with garbage data). | |
SO2 (const Ref< const Vec2T > &arr) | |
Create a transform from an array representing all rotation fields in \(\boldsymbol{q}\in\mathbb{R}^2\). | |
SO2 (const SO2 &q) | |
Copy constructor from another rotation. | |
SO2 (const T *data) | |
Create a rotation from a pointer array representing all rotation fields in \(\boldsymbol{q}\in\mathbb{R}^2\). | |
T & | operator[] (int i) |
Access a field from \(\boldsymbol{q}\in\mathbb{R}^2\). | |
const T & | w () const |
Access the real element of the rotation. | |
const T & | x () const |
Access the imaginary element of the rotation. | |
T & | w () |
Access the real element of the rotation. | |
T & | x () |
Access the imaginary element of the rotation. | |
const Vec2T | elements () const |
Access all elements of \(\boldsymbol{q}\in\mathbb{R}^2\). | |
Vec2T | array () const |
Access all elements of \(\boldsymbol{q}\in\mathbb{R}^2\). | |
T * | data () |
Access pointer to all elements of \(\boldsymbol{q}\in\mathbb{R}^2\). | |
const T * | data () const |
Access pointer to all elements of \(\boldsymbol{q}\in\mathbb{R}^2\). | |
SO2 | copy () const |
Get a deep copy of the current rotation. | |
void | normalize () |
Normalize the elements of \(\boldsymbol{q}\) to make it a valid rotation. | |
SO2 | normalized () |
Obtain a normalized copy of the current rotation. | |
Mat2T | R () const |
Convert the rotation to matrix representation \(\begin{bmatrix}\cos\theta & -\sin\theta
\\ \sin\theta & \cos\theta\end{bmatrix}\). | |
SO2 | inverse () const |
Obtain the inverse rotation \(\boldsymbol{R}_A^B\rightarrow \boldsymbol{R}_B^A\). | |
SO2 & | invert () |
Invert the current rotation \(\boldsymbol{R}_A^B\rightarrow \boldsymbol{R}_B^A\). | |
T | angle () const |
Obtain the equivalent scalar angle \(\theta\) of the rotation. | |
template<typename Tout = T, typename T2 > | |
SO2< Tout > | otimes (const SO2< T2 > &q) const |
Implementation of group composition: \(\boldsymbol{q}_B^C \otimes \boldsymbol{q}_A^B\rightarrow
\boldsymbol{q}_A^C\). | |
template<typename Tout = T, typename T2 > | |
SO2< Tout > | oplus (const Matrix< T2, 1, 1 > &delta) const |
Implementation of tangent space group perturbations: \(\boldsymbol{q}_A^B\oplus
\boldsymbol{\theta}_B^{B'} \rightarrow \boldsymbol{q}_A^{B'}\). | |
template<typename Tout = T, typename T2 > | |
Matrix< Tout, 1, 1 > | ominus (const SO2< T2 > &q) const |
Implementation of group subtraction: \(\boldsymbol{q}_A^B\ominus \boldsymbol{q}_A^{B'} \rightarrow
\boldsymbol{\theta}_B^{B'}\). | |
SO2 & | operator= (const SO2 &q) |
Copy constructor. | |
template<typename T2 > | |
SO2 | operator* (const SO2< T2 > &q) const |
Invocation of otimes via multiplication. | |
template<typename T2 > | |
SO2 & | operator*= (const SO2< T2 > &q) |
Invocation of otimes via multiplication. | |
SO2 & | operator*= (const double &s) |
Scale a rotation by a scalar. | |
SO2 & | operator/= (const double &s) |
Scale a rotation by a scalar. | |
SO2 | operator/ (const double &s) const |
Scale a rotation by a scalar. | |
template<typename Tout = T, typename T2 > | |
Matrix< Tout, 2, 1 > | operator* (const Matrix< T2, 2, 1 > &v) const |
Transform a vector via multiplication: \(\boldsymbol{q}_A^B\boldsymbol{t}^A \rightarrow
\boldsymbol{t}^B\). | |
Vec2T | operator* (const Vec2T &v) const |
Transform a vector via multiplication: \(\boldsymbol{q}_A^B\boldsymbol{t}^A \rightarrow
\boldsymbol{t}^B\). | |
SO2 | operator+ (const Vec1T &v) const |
Invocation of oplus via addition. | |
SO2 & | operator+= (const Vec1T &v) |
Invocation of oplus via addition. | |
template<typename T2 > | |
Vec1T | operator- (const SO2< T2 > &q) const |
Invocation of ominus via subtraction. | |
template<typename T2 > | |
SO2< T2 > | cast () const |
Cast the underlying numeric type. | |
Static Public Member Functions | |
static SO2 | random () |
Obtain a random rotation. | |
static SO2 | identity () |
Obtain an identity \(SO(2)\) rotation. | |
static SO2 | nans () |
Obtain a rotation full of NaNs. | |
static SO2 | fromAngle (const T angle) |
Convert an angle (in radians) into a rotation. | |
static SO2 | fromR (const Mat2T &m) |
Convert a rotation matrix \(\boldsymbol{R}\in\mathbb{R}^{2\times 2}\) to a \(SO(2)\) rotation. | |
static SO2 | fromTwoUnitVectors (const Vec2T &u, const Vec2T &v) |
Given two unit vectors \(\boldsymbol{u},\boldsymbol{v}\in \mathbb{R}^2\), returns the rotation that rotates \(\boldsymbol{u}\rightarrow\boldsymbol{v}\). | |
static SO2 | fromComplex (const T qw, const T qx) |
Construct a rotation from the individual fields. | |
static SO2 | fromComplex (const Vec2T &qvec) |
Construct a rotation from a rotation fields vector. | |
static Mat2T | hat (const Vec1T &omega) |
Hat operator implementation, which coverts the tangent-space vector representation to the corresponding Lie algebra: \(\mathbb{R}\rightarrow \mathfrak{so}(2)\). | |
static Vec1T | vee (const Mat2T &Omega) |
Vee operator implementation, which coverts the Lie algebra representation to a tangent-space vector representation: \(\mathfrak{so}(2) \rightarrow \mathbb{R}\). | |
static Mat2T | log (const SO2 &q) |
Logarithmic chart map implementation: \(SO(2) \rightarrow \mathfrak{so}(2)\). | |
static Vec1T | Log (const SO2 &q) |
Logarithmic chart map implementation: \(SO(2) \rightarrow \mathbb{R}\). | |
static SO2 | exp (const Mat2T &Omega) |
Exponential chart map implementation: \(\mathfrak{so}(2) \rightarrow SO(2)\). | |
static SO2 | Exp (const Vec1T &omega) |
Exponential chart map implementation: \(\mathbb{R} \rightarrow SO(2)\). | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Map< Vec2T > | arr_ |
Memory-mapped array representing all rotation fields in \(\boldsymbol{q}\). | |
Class representing a member of the \(SO(2)\) manifold, or a 2D rotation.
Create a transform from an array representing all rotation fields in \(\boldsymbol{q}\in\mathbb{R}^2\).
Copy constructor from another rotation.
Create a rotation from a pointer array representing all rotation fields in \(\boldsymbol{q}\in\mathbb{R}^2\).
Obtain the equivalent scalar angle \(\theta\) of the rotation.
\(-\pi \leq \theta \leq \pi\).
Access all elements of \(\boldsymbol{q}\in\mathbb{R}^2\).
Cast the underlying numeric type.
Access pointer to all elements of \(\boldsymbol{q}\in\mathbb{R}^2\).
Access pointer to all elements of \(\boldsymbol{q}\in\mathbb{R}^2\).
Access all elements of \(\boldsymbol{q}\in\mathbb{R}^2\).
Exponential chart map implementation: \(\mathfrak{so}(2) \rightarrow SO(2)\).
Exponential chart map implementation: \(\mathbb{R} \rightarrow SO(2)\).
Convert an angle (in radians) into a rotation.
Construct a rotation from the individual fields.
qw | Real component of the rotation \(\boldsymbol{q}\in SO(2)\). |
qx | Imaginary component of the rotation \(\boldsymbol{q}\in SO(2)\). |
Construct a rotation from a rotation fields vector.
qvec | The rotation represented as an array \(\boldsymbol{q}\in\mathbb{R}^2\). |
Convert a rotation matrix \(\boldsymbol{R}\in\mathbb{R}^{2\times 2}\) to a \(SO(2)\) rotation.
Given two unit vectors \(\boldsymbol{u},\boldsymbol{v}\in \mathbb{R}^2\), returns the rotation that rotates \(\boldsymbol{u}\rightarrow\boldsymbol{v}\).
Hat operator implementation, which coverts the tangent-space vector representation to the corresponding Lie algebra: \(\mathbb{R}\rightarrow \mathfrak{so}(2)\).
Obtain an identity \(SO(2)\) rotation.
Obtain the inverse rotation \(\boldsymbol{R}_A^B\rightarrow \boldsymbol{R}_B^A\).
Invert the current rotation \(\boldsymbol{R}_A^B\rightarrow \boldsymbol{R}_B^A\).
Logarithmic chart map implementation: \(SO(2) \rightarrow \mathfrak{so}(2)\).
Logarithmic chart map implementation: \(SO(2) \rightarrow \mathbb{R}\).
Normalize the elements of \(\boldsymbol{q}\) to make it a valid rotation.
Obtain a normalized copy of the current rotation.
Implementation of group subtraction: \(\boldsymbol{q}_A^B\ominus \boldsymbol{q}_A^{B'} \rightarrow \boldsymbol{\theta}_B^{B'}\).
Transform a vector via multiplication: \(\boldsymbol{q}_A^B\boldsymbol{t}^A \rightarrow \boldsymbol{t}^B\).
Invocation of otimes via multiplication.
Transform a vector via multiplication: \(\boldsymbol{q}_A^B\boldsymbol{t}^A \rightarrow \boldsymbol{t}^B\).
Scale a rotation by a scalar.
Under the hood, this converts the rotation into a tangent-space vector, scales the vector, then converts the scaled vector back to a rotation.
Invocation of oplus via addition.
Invocation of oplus via addition.
Scale a rotation by a scalar.
Under the hood, this converts the rotation into a tangent-space vector, scales the vector, then converts the scaled vector back to a rotation.
Scale a rotation by a scalar.
Under the hood, this converts the rotation into a tangent-space vector, scales the vector, then converts the scaled vector back to a rotation.
Access a field from \(\boldsymbol{q}\in\mathbb{R}^2\).
Implementation of tangent space group perturbations: \(\boldsymbol{q}_A^B\oplus \boldsymbol{\theta}_B^{B'} \rightarrow \boldsymbol{q}_A^{B'}\).
|
inline |
Implementation of group composition: \(\boldsymbol{q}_B^C \otimes \boldsymbol{q}_A^B\rightarrow \boldsymbol{q}_A^C\).
Convert the rotation to matrix representation \(\begin{bmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta\end{bmatrix}\).
Obtain a random rotation.
The rotation \(\mathbf{q}_B^W\) will be normalized.
Vee operator implementation, which coverts the Lie algebra representation to a tangent-space vector representation: \(\mathfrak{so}(2) \rightarrow \mathbb{R}\).
Access the real element of the rotation.
Access the imaginary element of the rotation.
Memory-mapped array representing all rotation fields in \(\boldsymbol{q}\).