#include <SOn.h>
Classes | |
struct | ChartAtOrigin |
Public Types | |
enum | { dimension = internal::DimensionSO(N) } |
using | MatrixDD = Eigen::Matrix< double, dimension, dimension > |
using | MatrixNN = Eigen::Matrix< double, N, N > |
using | VectorN2 = Eigen::Matrix< double, internal::NSquaredSO(N), 1 > |
Public Types inherited from gtsam::LieGroup< SO< N >, internal::DimensionSO(N)> | |
enum | |
typedef OptionalJacobian< N, N > | ChartJacobian |
typedef Eigen::Matrix< double, N, N > | Jacobian |
typedef Eigen::Matrix< double, N, 1 > | TangentVector |
Public Member Functions | |
Standard methods | |
const MatrixNN & | matrix () const |
Return matrix. More... | |
size_t | rows () const |
size_t | cols () const |
Testable | |
void | print (const std::string &s=std::string()) const |
bool | equals (const SO &other, double tol) const |
Public Member Functions inherited from gtsam::LieGroup< SO< N >, internal::DimensionSO(N)> | |
SO< N > | between (const SO< N > &g) const |
SO< N > | between (const SO< N > &g, ChartJacobian H1, ChartJacobian H2={}) const |
SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
GTSAM_EXPORT SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
SO< N > | compose (const SO< N > &g) const |
SO< N > | compose (const SO< N > &g, ChartJacobian H1, ChartJacobian H2={}) const |
SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
GTSAM_EXPORT SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
const SO< N > & | derived () const |
SO< N > | expmap (const TangentVector &v) const |
SO< N > | expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
expmap with optional derivatives More... | |
SO< N > | inverse (ChartJacobian H) const |
TangentVector | localCoordinates (const SO< N > &g) const |
localCoordinates as required by manifold concept: finds tangent vector between *this and g More... | |
TangentVector | localCoordinates (const SO< N > &g, ChartJacobian H1, ChartJacobian H2={}) const |
localCoordinates with optional derivatives More... | |
TangentVector | logmap (const SO< N > &g) const |
TangentVector | logmap (const SO< N > &g, ChartJacobian H1, ChartJacobian H2={}) const |
logmap with optional derivatives More... | |
SO< N > | retract (const TangentVector &v) const |
retract as required by manifold concept: applies v at *this More... | |
SO< N > | retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
retract with optional derivatives More... | |
Protected Types | |
template<int N_> | |
using | IsDynamic = typename std::enable_if< N_==Eigen::Dynamic, void >::type |
template<int N_> | |
using | IsFixed = typename std::enable_if< N_ >=2, void >::type |
template<int N_> | |
using | IsSO3 = typename std::enable_if< N_==3, void >::type |
Protected Attributes | |
MatrixNN | matrix_ |
Rotation matrix. More... | |
Constructors | |
template<int N_ = N, typename = IsFixed<N_>> | |
SO () | |
Construct SO<N> identity for N >= 2. More... | |
template<int N_ = N, typename = IsDynamic<N_>> | |
SO (size_t n=0) | |
Construct SO<N> identity for N == Eigen::Dynamic. More... | |
template<typename Derived > | |
SO (const Eigen::MatrixBase< Derived > &R) | |
Constructor from Eigen Matrix, dynamic version. More... | |
template<int M, int N_ = N, typename = IsDynamic<N_>> | |
SO (const SO< M > &R) | |
Construct dynamic SO(n) from Fixed SO<M> More... | |
template<int N_ = N, typename = IsSO3<N_>> | |
SO (const Eigen::AngleAxisd &angleAxis) | |
Constructor from AngleAxisd. More... | |
template<typename Derived > | |
static SO | FromMatrix (const Eigen::MatrixBase< Derived > &R) |
Named constructor from Eigen Matrix. More... | |
template<typename Derived , int N_ = N, typename = IsDynamic<N_>> | |
static SO | Lift (size_t n, const Eigen::MatrixBase< Derived > &R) |
Named constructor from lower dimensional matrix. More... | |
static SO | AxisAngle (const Vector3 &axis, double theta) |
Constructor from axis and angle. Only defined for SO3. More... | |
static SO | ClosestTo (const MatrixNN &M) |
static SO | ChordalMean (const std::vector< SO > &rotations) |
template<int N_ = N, typename = IsDynamic<N_>> | |
static SO | Random (std::mt19937 &rng, size_t n=0) |
Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3.cpp. More... | |
template<int N_ = N, typename = IsFixed<N_>> | |
static SO | Random (std::mt19937 &rng) |
Random SO(N) element (no big claims about uniformity) More... | |
Group | |
SO | operator* (const SO &other) const |
Multiplication. More... | |
SO | inverse () const |
inverse of a rotation = transpose More... | |
template<int N_ = N, typename = IsFixed<N_>> | |
static SO | Identity () |
SO<N> identity for N >= 2. More... | |
template<int N_ = N, typename = IsDynamic<N_>> | |
static SO | Identity (size_t n=0) |
SO<N> identity for N == Eigen::Dynamic. More... | |
Manifold | |
using | TangentVector = Eigen::Matrix< double, dimension, 1 > |
using | ChartJacobian = OptionalJacobian< dimension, dimension > |
size_t | dim () const |
static int | Dim () |
Return compile-time dimensionality: fixed size N or Eigen::Dynamic. More... | |
static size_t | Dimension (size_t n) |
static size_t | AmbientDim (size_t d) |
static MatrixNN | Hat (const TangentVector &xi) |
static void | Hat (const Vector &xi, Eigen::Ref< MatrixNN > X) |
In-place version of Hat (see details there), implements recursion. More... | |
static TangentVector | Vee (const MatrixNN &X) |
Inverse of Hat. See note about xi element order in Hat. More... | |
template<int N_ = N, typename = IsDynamic<N_>> | |
static MatrixDD | IdentityJacobian (size_t n) |
Lie Group | |
MatrixDD | AdjointMap () const |
Adjoint map. More... | |
static SO | Expmap (const TangentVector &omega, ChartJacobian H={}) |
static MatrixDD | ExpmapDerivative (const TangentVector &omega) |
Derivative of Expmap, currently only defined for SO3. More... | |
static TangentVector | Logmap (const SO &R, ChartJacobian H={}) |
static MatrixDD | LogmapDerivative (const TangentVector &omega) |
Derivative of Logmap, currently only defined for SO3. More... | |
Other methods | |
VectorN2 | vec (OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const |
template<int N_ = N, typename = IsFixed<N_>> | |
static Matrix | VectorizedGenerators () |
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) More... | |
template<int N_ = N, typename = IsDynamic<N_>> | |
static Matrix | VectorizedGenerators (size_t n=0) |
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n) More... | |
Additional Inherited Members | |
Static Public Member Functions inherited from gtsam::LieGroup< SO< N >, internal::DimensionSO(N)> | |
static TangentVector | LocalCoordinates (const SO< N > &g) |
LocalCoordinates at origin: possible in Lie group because it has an identity. More... | |
static TangentVector | LocalCoordinates (const SO< N > &g, ChartJacobian H) |
LocalCoordinates at origin with optional derivative. More... | |
static SO< N > | Retract (const TangentVector &v) |
Retract at origin: possible in Lie group because it has an identity. More... | |
static SO< N > | Retract (const TangentVector &v, ChartJacobian H) |
Retract at origin with optional derivative. More... | |
Manifold of special orthogonal rotation matrices SO<N>. Template paramater N can be a fixed integer or can be Eigen::Dynamic
using gtsam::SO< N >::ChartJacobian = OptionalJacobian<dimension, dimension> |
|
protected |
using gtsam::SO< N >::TangentVector = Eigen::Matrix<double, dimension, 1> |
using gtsam::SO< N >::VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1> |
|
inlineexplicit |
Construct SO<N> identity for N == Eigen::Dynamic.
|
inlineexplicit |
|
inlineexplicit |
Constructor from axis and angle. Only defined for SO3.
|
static |
Named constructor that finds chordal mean , currently only defined for SO3.
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for SO3.
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
|
static |
|
inlinestatic |
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of the manifold. This function is implemented recursively, and the d-vector is assumed to laid out such that the last element corresponds to so(2), the last 3 to so(3), the last 6 to so(4) etc... For example, the vector-space isomorphic to so(5) is laid out as: a b c d | u v w | x y | z where the latter elements correspond to "telescoping" sub-algebras: 0 -z y w -d z 0 -x -v c -y x 0 u -b -w v -u 0 a d -c b -a 0 This scheme behaves exactly as expected for SO(2) and SO(3).
|
static |
In-place version of Hat (see details there), implements recursion.
|
inlinestatic |
SO<N> identity for N == Eigen::Dynamic.
|
inlinestatic |
|
static |
SO< N >::VectorN2 gtsam::SO< N >::vec | ( | OptionalJacobian< internal::NSquaredSO< N >(N), dimension > | H = {} | ) | const |