MRPT  1.9.9
SO.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/optional_ref.h>
12 #include <mrpt/math/CMatrixFixed.h>
13 #include <mrpt/math/CVectorFixed.h>
14 
15 namespace mrpt::poses::Lie
16 {
17 /** Traits for SO(n), rotations in R^n space.
18  * \ingroup mrpt_poses_lie_grp
19  */
20 template <unsigned int n>
21 struct SO;
22 
23 /** Traits for SO(3), rotations in R^3 space.
24  * \ingroup mrpt_poses_lie_grp
25  */
26 template <>
27 struct SO<3>
28 {
29  constexpr static size_t DOFs = 3;
32 
33  /** Type for Jacobian: tangent space -> SO(n) matrix */
35 
36  /** Type for Jacobian: SO(n) matrix -> tangent space */
38 
39  /** SO(3) exponential map \f$ x \rightarrow \exp(x^\wedge) \f$.
40  * This is exactly the same than the Rodrigues formula.
41  * See 9.4.1 in \cite blanco_se3_tutorial for the exponential map
42  * definition.
43  *
44  * - Input: 3-len vector in Lie algebra so(3)
45  * - Output: 3x3 rotation matrix in SO(3)
46  */
47  static type exp(const tangent_vector& x);
48 
49  /** SO(3) logarithm map \f$ \mathbf{R} \rightarrow \log(\mathbf{R}^\vee)\f$.
50  * See 10.3.1 in \cite blanco_se3_tutorial
51  *
52  * - Input: 3x3 rotation matrix in SO(3)
53  * - Output: 3-len vector in Lie algebra so(3)
54  */
55  static tangent_vector log(const type& R);
56 
57  /** Returns the 3x3 SO(3) rotation matrix from yaw, pitch, roll angles.
58  * See CPose3D for the axis conventions and a picture. */
59  static type fromYPR(
60  const double yaw, const double pitch, const double roll);
61 
62  /** Returns vee(R-R'), which is an approximation to 2*vee(logmat(R)) for
63  * small rotations. */
64  static tangent_vector vee_RmRt(const type& R);
65 
66  /** Jacobian for exp(). See 10.3.1 in \cite blanco_se3_tutorial
67  *
68  * - Input: 3-len vector in Lie algebra so(3)
69  * - Jacobian: Jacobian of the 3x3 matrix (stacked as a column major
70  * 9-vector) wrt the vector in the tangent space.
71  */
72  static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);
73 
74  /** Jacobian for log()
75  * See 10.3.2 in \cite blanco_se3_tutorial
76  *
77  * - Input: 3x3 rotation matrix in SO(3)
78  * - Jacobian: Jacobian of the tangent space vector wrt the 3x3 matrix
79  * (stacked as a column major 9-vector).
80  */
81  static mat2tang_jacob jacob_dlogv_dv(const type& R);
82 };
83 
84 /** Traits for SO(2), rotations in R^2 space.
85  * \ingroup mrpt_poses_lie_grp
86  */
87 template <>
88 struct SO<2>
89 {
90  constexpr static size_t DOFs = 1;
92  using type = double;
93 
94  /** Type for Jacobian: tangent space to SO(n) matrix */
96  /** Type for Jacobian: SO(n) matrix to tangent space */
98 
99  /** SO(2) exponential map \f$ x \rightarrow \exp(x^\wedge) \f$.
100  * - Input: 1-len vector in Lie algebra so(3)
101  * - Output: angle for the rotation (radians)
102  */
103  static type exp(const tangent_vector& x);
104 
105  /** SO(2) logarithm map \f$ \mathbf{R} \rightarrow \log(\mathbf{R}^\vee)\f$.
106  * - Input: rotation angle [radians]
107  * - Output: 1-len vector in Lie algebra so(2) (with the same value)
108  */
109  static tangent_vector log(const type& R);
110 
111  /** Jacobian for exp(), the identity matrix `[ 1 ]` */
112  static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);
113 
114  /** Jacobian for log(), the identity matrix `[ 1 ]` */
115  static mat2tang_jacob jacob_dlogv_dv(const type& R);
116 };
117 
118 } // namespace mrpt::poses::Lie
CMatrixFixed< double, 3, 9 > CMatrixDouble39
Definition: CMatrixFixed.h:387
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
CMatrixFixed< double, 9, 3 > CMatrixDouble93
Definition: CMatrixFixed.h:388
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
const float R
Traits for SO(n), rotations in R^n space.
Definition: SO.h:21



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