MRPT  1.9.9
Classes | Typedefs | Functions | Variables
Lie Algebra methods for SO(2),SO(3),SE(2),SE(3)

Detailed Description

Collaboration diagram for Lie Algebra methods for SO(2),SO(3),SE(2),SE(3):

Classes

struct  mrpt::poses::Lie::Euclidean< N >
 Traits for Euclidean R^N space. More...
 
struct  mrpt::poses::Lie::SE< n >
 Traits for SE(n), rigid-body transformations in R^n space. More...
 
struct  mrpt::poses::Lie::SE< 3 >
 Traits for SE(3), rigid-body transformations in R^3 space. More...
 
struct  mrpt::poses::Lie::SE< 2 >
 Traits for SE(2), rigid-body transformations in R^2 space. More...
 
struct  mrpt::poses::Lie::SO< n >
 Traits for SO(n), rotations in R^n space. More...
 
struct  mrpt::poses::Lie::SO< 3 >
 Traits for SO(3), rotations in R^3 space. More...
 
struct  mrpt::poses::Lie::SO< 2 >
 Traits for SO(2), rotations in R^2 space. More...
 

Typedefs

using mrpt::poses::Lie::SE< 3 >::tangent_vector = mrpt::math::CVectorFixedDouble< DOFs >
 
using mrpt::poses::Lie::SE< 3 >::manifold_vector = mrpt::math::CVectorFixedDouble< MANIFOLD_DIM >
 
using mrpt::poses::Lie::SE< 3 >::type = CPose3D
 
using mrpt::poses::Lie::SE< 3 >::light_type = mrpt::math::TPose3D
 
using mrpt::poses::Lie::SE< 3 >::tang2mat_jacob = mrpt::math::CMatrixDouble12_6
 Type for Jacobian: tangent space -> SO(n) matrix. More...
 
using mrpt::poses::Lie::SE< 3 >::mat2tang_jacob = mrpt::math::CMatrixDouble6_12
 Type for Jacobian: SO(n) matrix -> tangent space. More...
 
using mrpt::poses::Lie::SE< 3 >::matrix_TxT = mrpt::math::CMatrixDouble66
 Type for Jacobians between so(n) vectors on the tangent space. More...
 
using mrpt::poses::Lie::SE< 3 >::matrix_MxM = mrpt::math::CMatrixFixed< double, 12, 12 >
 Type for Jacobians between SO(n) 3x4 (sub)matrices in the manifold. More...
 
using mrpt::poses::Lie::SE< 2 >::tangent_vector = mrpt::math::CVectorFixedDouble< DOFs >
 
using mrpt::poses::Lie::SE< 2 >::manifold_vector = mrpt::math::CVectorFixedDouble< MANIFOLD_DIM >
 
using mrpt::poses::Lie::SE< 2 >::type = CPose2D
 
using mrpt::poses::Lie::SE< 2 >::light_type = mrpt::math::TPose2D
 
using mrpt::poses::Lie::SE< 2 >::matrix_TxT = mrpt::math::CMatrixDouble33
 Type for Jacobians between SO(n) transformations. More...
 
using mrpt::poses::Lie::SE< 2 >::matrix_MxM = mrpt::math::CMatrixDouble33
 In SE(3), this type represents Jacobians between SO(n) (sub)matrices in the manifold, but in SE(2) it simply models Jacobians between (x,y,phi) vectors. More...
 
using mrpt::poses::Lie::SE< 2 >::tang2mat_jacob = mrpt::math::CMatrixDouble33
 Type for Jacobian: tangent space -> SO(n) matrix. More...
 
using mrpt::poses::Lie::SE< 2 >::mat2tang_jacob = mrpt::math::CMatrixDouble33
 Type for Jacobian: SO(n) matrix -> tangent space. More...
 

Functions

static type mrpt::poses::Lie::SE< 3 >::exp (const tangent_vector &x)
 Retraction to SE(3), a pseudo-exponential map $ x \rightarrow PseudoExp(x^\wedge) $ and its Jacobian. More...
 
static tangent_vector mrpt::poses::Lie::SE< 3 >::log (const type &P)
 SE(3) pseudo-logarithm map $ \mathbf{R} \rightarrow \log(\mathbf{R}^\vee)$. More...
 
static manifold_vector mrpt::poses::Lie::SE< 3 >::asManifoldVector (const type &pose)
 Returns a vector with all manifold matrix elements in column-major order. More...
 
static type mrpt::poses::Lie::SE< 3 >::fromManifoldVector (const manifold_vector &v)
 The inverse operation of asManifoldVector() More...
 
static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dexpe_de (const tangent_vector &x)
 Jacobian for the pseudo-exponential exp(). More...
 
static mat2tang_jacob mrpt::poses::Lie::SE< 3 >::jacob_dlogv_dv (const type &P)
 Jacobian for the pseudo-logarithm log() See 10.3.11 in [1]. More...
 
static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dexpeD_de (const CPose3D &D)
 Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra. More...
 
static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dDexpe_de (const CPose3D &D)
 Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra. More...
 
static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dAexpeD_de (const CPose3D &A, const CPose3D &D)
 Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra. More...
 
static matrix_MxM mrpt::poses::Lie::SE< 3 >::jacob_dAB_dA (const type &A, const type &B)
 Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices, with respect to A. More...
 
static matrix_MxM mrpt::poses::Lie::SE< 3 >::jacob_dAB_dB (const type &A, const type &B)
 Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices, with respect to B. More...
 
static void mrpt::poses::Lie::SE< 3 >::jacob_dDinvP1invP2_de1e2 (const type &Dinv, const type &P1, const type &P2, mrpt::optional_ref< matrix_TxT > df_de1=std::nullopt, mrpt::optional_ref< matrix_TxT > df_de2=std::nullopt)
 Return one or both of the following 6x6 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\]

With $ \epsilon_1 $ and $ \epsilon_2 $ increments in the linearized manifold for P1 and P2. More...

 
static type mrpt::poses::Lie::SE< 2 >::exp (const tangent_vector &x)
 Exponential map in SE(2), takes [x,y,phi] and returns a CPose2D. More...
 
static tangent_vector mrpt::poses::Lie::SE< 2 >::log (const type &P)
 Logarithm map in SE(2), takes a CPose2D and returns [X,Y, phi]. More...
 
static manifold_vector mrpt::poses::Lie::SE< 2 >::asManifoldVector (const type &pose)
 Returns a vector with all manifold matrix elements in column-major order. More...
 
static type mrpt::poses::Lie::SE< 2 >::fromManifoldVector (const manifold_vector &v)
 The inverse operation of asManifoldVector() More...
 
static matrix_MxM mrpt::poses::Lie::SE< 2 >::jacob_dAB_dA (const type &A, const type &B)
 Jacobian of the pose composition A*B for SE(2) with respect to A. More...
 
static matrix_MxM mrpt::poses::Lie::SE< 2 >::jacob_dAB_dB (const type &A, const type &B)
 Jacobian of the pose composition A*B for SE(2) with respect to B. More...
 
static tang2mat_jacob mrpt::poses::Lie::SE< 2 >::jacob_dDexpe_de (const type &D)
 Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra. More...
 
static void mrpt::poses::Lie::SE< 2 >::jacob_dDinvP1invP2_de1e2 (const type &Dinv, const type &P1, const type &P2, mrpt::optional_ref< matrix_TxT > df_de1=std::nullopt, mrpt::optional_ref< matrix_TxT > df_de2=std::nullopt)
 Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\]

With $ \epsilon_1 $ and $ \epsilon_2 $ increments in the linearized manifold for P1 and P2. More...

 

Variables

static constexpr size_t mrpt::poses::Lie::SE< 3 >::DOFs = 6
 Number of actual degrees of freedom for this transformation. More...
 
static constexpr size_t mrpt::poses::Lie::SE< 3 >::MANIFOLD_DIM = 3 * 4
 Dimensionality of the matrix manifold (3x4=12 upper part of the 4x4) More...
 
static constexpr size_t mrpt::poses::Lie::SE< 2 >::DOFs = 3
 Number of actual degrees of freedom for this transformation. More...
 
static constexpr size_t mrpt::poses::Lie::SE< 2 >::MANIFOLD_DIM = 3
 Dimensionality of the matrix manifold; this should be 3x3=9 for SE(2), but for efficiency, we define it as simply 3x1=3 and use the (x,y,phi) representation as well as the "manifold". More...
 

Typedef Documentation

◆ light_type [1/2]

Definition at line 50 of file SE.h.

◆ light_type [2/2]

Definition at line 177 of file SE.h.

◆ manifold_vector [1/2]

Definition at line 47 of file SE.h.

◆ manifold_vector [2/2]

Definition at line 174 of file SE.h.

◆ mat2tang_jacob [1/2]

Type for Jacobian: SO(n) matrix -> tangent space.

Definition at line 56 of file SE.h.

◆ mat2tang_jacob [2/2]

Type for Jacobian: SO(n) matrix -> tangent space.

Definition at line 190 of file SE.h.

◆ matrix_MxM [1/2]

Type for Jacobians between SO(n) 3x4 (sub)matrices in the manifold.

Definition at line 62 of file SE.h.

◆ matrix_MxM [2/2]

In SE(3), this type represents Jacobians between SO(n) (sub)matrices in the manifold, but in SE(2) it simply models Jacobians between (x,y,phi) vectors.

Definition at line 185 of file SE.h.

◆ matrix_TxT [1/2]

Type for Jacobians between so(n) vectors on the tangent space.

Definition at line 59 of file SE.h.

◆ matrix_TxT [2/2]

Type for Jacobians between SO(n) transformations.

Definition at line 180 of file SE.h.

◆ tang2mat_jacob [1/2]

Type for Jacobian: tangent space -> SO(n) matrix.

Definition at line 53 of file SE.h.

◆ tang2mat_jacob [2/2]

Type for Jacobian: tangent space -> SO(n) matrix.

Definition at line 188 of file SE.h.

◆ tangent_vector [1/2]

Definition at line 46 of file SE.h.

◆ tangent_vector [2/2]

Definition at line 173 of file SE.h.

◆ type [1/2]

Definition at line 49 of file SE.h.

◆ type [2/2]

Definition at line 176 of file SE.h.

Function Documentation

◆ asManifoldVector() [1/2]

static manifold_vector mrpt::poses::Lie::SE< 3 >::asManifoldVector ( const type pose)
static

Returns a vector with all manifold matrix elements in column-major order.

For SE(3), it is a 3x4=12 vector.

◆ asManifoldVector() [2/2]

static manifold_vector mrpt::poses::Lie::SE< 2 >::asManifoldVector ( const type pose)
static

Returns a vector with all manifold matrix elements in column-major order.

For SE(2), though, it directly returns the vector [x,y,phi] for efficiency in comparison to 3x3 homogeneous coordinates.

◆ exp() [1/2]

static type mrpt::poses::Lie::SE< 3 >::exp ( const tangent_vector x)
static

Retraction to SE(3), a pseudo-exponential map $ x \rightarrow PseudoExp(x^\wedge) $ and its Jacobian.

  • Input: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
  • Output: translation and rotation in SE(3) as CPose3D Note that this method implements retraction via a pseudo-exponential, where only the rotational part undergoes a real matrix exponential, while the translation is left unmodified. This is done for computational efficiency, and does not change the results of optimizations as long as the corresponding local coordinates (pseudo-logarithm) are used as well.

See section 9.4.2 in [1]

◆ exp() [2/2]

static type mrpt::poses::Lie::SE< 2 >::exp ( const tangent_vector x)
static

Exponential map in SE(2), takes [x,y,phi] and returns a CPose2D.

◆ fromManifoldVector() [1/2]

static type mrpt::poses::Lie::SE< 3 >::fromManifoldVector ( const manifold_vector v)
static

The inverse operation of asManifoldVector()

◆ fromManifoldVector() [2/2]

static type mrpt::poses::Lie::SE< 2 >::fromManifoldVector ( const manifold_vector v)
static

The inverse operation of asManifoldVector()

◆ jacob_dAB_dA() [1/2]

static matrix_MxM mrpt::poses::Lie::SE< 3 >::jacob_dAB_dA ( const type A,
const type B 
)
static

Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices, with respect to A.

Note
See section 7.3.1 of in [1]

◆ jacob_dAB_dA() [2/2]

static matrix_MxM mrpt::poses::Lie::SE< 2 >::jacob_dAB_dA ( const type A,
const type B 
)
static

Jacobian of the pose composition A*B for SE(2) with respect to A.

Note
See appendix B of [1]

◆ jacob_dAB_dB() [1/2]

static matrix_MxM mrpt::poses::Lie::SE< 3 >::jacob_dAB_dB ( const type A,
const type B 
)
static

Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices, with respect to B.

Note
See section 7.3.1 of in [1]

◆ jacob_dAB_dB() [2/2]

static matrix_MxM mrpt::poses::Lie::SE< 2 >::jacob_dAB_dB ( const type A,
const type B 
)
static

Jacobian of the pose composition A*B for SE(2) with respect to B.

Note
See appendix B of [1]

◆ jacob_dAexpeD_de()

static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dAexpeD_de ( const CPose3D A,
const CPose3D D 
)
static

Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.

Note
Eq. 10.3.7 in [1]

◆ jacob_dDexpe_de() [1/2]

static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dDexpe_de ( const CPose3D D)
static

Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.

Note
Section 10.3.4 in [1]

◆ jacob_dDexpe_de() [2/2]

static tang2mat_jacob mrpt::poses::Lie::SE< 2 >::jacob_dDexpe_de ( const type D)
static

Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.

Note
See appendix B in [1]

◆ jacob_dDinvP1invP2_de1e2() [1/2]

static void mrpt::poses::Lie::SE< 3 >::jacob_dDinvP1invP2_de1e2 ( const type Dinv,
const type P1,
const type P2,
mrpt::optional_ref< matrix_TxT df_de1 = std::nullopt,
mrpt::optional_ref< matrix_TxT df_de2 = std::nullopt 
)
static

Return one or both of the following 6x6 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\]

With $ \epsilon_1 $ and $ \epsilon_2 $ increments in the linearized manifold for P1 and P2.

Note
Section 10.3.10 in [1]

◆ jacob_dDinvP1invP2_de1e2() [2/2]

static void mrpt::poses::Lie::SE< 2 >::jacob_dDinvP1invP2_de1e2 ( const type Dinv,
const type P1,
const type P2,
mrpt::optional_ref< matrix_TxT df_de1 = std::nullopt,
mrpt::optional_ref< matrix_TxT df_de2 = std::nullopt 
)
static

Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\]

With $ \epsilon_1 $ and $ \epsilon_2 $ increments in the linearized manifold for P1 and P2.

Note
See appendix B in [1]

◆ jacob_dexpe_de()

static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dexpe_de ( const tangent_vector x)
static

Jacobian for the pseudo-exponential exp().

See 10.3.1 in [1]

  • Input: 6-len vector in Lie algebra se(3)
  • Jacobian: Jacobian of the 3x4 matrix (stacked as a column major 12-vector) wrt the vector in the tangent space.

◆ jacob_dexpeD_de()

static tang2mat_jacob mrpt::poses::Lie::SE< 3 >::jacob_dexpeD_de ( const CPose3D D)
static

Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.

Note
Section 10.3.3 in [1]

◆ jacob_dlogv_dv()

static mat2tang_jacob mrpt::poses::Lie::SE< 3 >::jacob_dlogv_dv ( const type P)
static

Jacobian for the pseudo-logarithm log() See 10.3.11 in [1].

  • Input: a SE(3) pose as a CPose3D object
  • Jacobian: Jacobian of the tangent space vector wrt the 3x4 matrix elements (stacked as a column major 12-vector).

◆ log() [1/2]

static tangent_vector mrpt::poses::Lie::SE< 3 >::log ( const type P)
static

SE(3) pseudo-logarithm map $ \mathbf{R} \rightarrow \log(\mathbf{R}^\vee)$.

  • Input: translation and rotation in SE(3) as CPose3D
  • Output: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]

See exp() for the explanation about the "pseudo" name. For the formulas, see section 9.4.2 in [1]

◆ log() [2/2]

static tangent_vector mrpt::poses::Lie::SE< 2 >::log ( const type P)
static

Logarithm map in SE(2), takes a CPose2D and returns [X,Y, phi].

Variable Documentation

◆ DOFs [1/2]

constexpr size_t mrpt::poses::Lie::SE< 3 >::DOFs = 6
static

Number of actual degrees of freedom for this transformation.

Definition at line 41 of file SE.h.

◆ DOFs [2/2]

constexpr size_t mrpt::poses::Lie::SE< 2 >::DOFs = 3
static

Number of actual degrees of freedom for this transformation.

Definition at line 165 of file SE.h.

◆ MANIFOLD_DIM [1/2]

constexpr size_t mrpt::poses::Lie::SE< 3 >::MANIFOLD_DIM = 3 * 4
static

Dimensionality of the matrix manifold (3x4=12 upper part of the 4x4)

Definition at line 44 of file SE.h.

◆ MANIFOLD_DIM [2/2]

constexpr size_t mrpt::poses::Lie::SE< 2 >::MANIFOLD_DIM = 3
static

Dimensionality of the matrix manifold; this should be 3x3=9 for SE(2), but for efficiency, we define it as simply 3x1=3 and use the (x,y,phi) representation as well as the "manifold".

This is done for API consistency with SE(3), where the actual matrix is used instead.

Definition at line 171 of file SE.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020