MRPT  2.0.1
SE.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 #include <mrpt/math/TPose2D.h>
15 #include <mrpt/math/TPose3D.h>
16 #include <mrpt/poses/poses_frwds.h>
17 
18 /** \defgroup mrpt_poses_lie_grp Lie Algebra methods for SO(2),SO(3),SE(2),SE(3)
19  * \ingroup poses_grp */
20 
21 namespace mrpt::poses::Lie
22 {
23 /** \addtogroup mrpt_poses_lie_grp
24  * @{ */
25 
26 /** Traits for SE(n), rigid-body transformations in R^n space.
27  * \ingroup mrpt_poses_lie_grp
28  */
29 template <unsigned int n>
30 struct SE;
31 
32 /** Traits for SE(3), rigid-body transformations in R^3 space.
33  * See indidual members for documentation, or \cite blanco_se3_tutorial for a
34  * general overview.
35  * \ingroup mrpt_poses_lie_grp
36  */
37 template <>
38 struct SE<3>
39 {
40  /** Number of actual degrees of freedom for this transformation */
41  constexpr static size_t DOFs = 6;
42 
43  /** Dimensionality of the matrix manifold (3x4=12 upper part of the 4x4) */
44  constexpr static size_t MANIFOLD_DIM = 3 * 4;
45 
48 
49  using type = CPose3D;
51 
52  /** Type for Jacobian: tangent space -> SO(n) matrix */
54 
55  /** Type for Jacobian: SO(n) matrix -> tangent space */
57 
58  /** Type for Jacobians between so(n) vectors on the tangent space */
60 
61  /** Type for Jacobians between SO(n) 3x4 (sub)matrices in the manifold */
63 
64  /** Retraction to SE(3), a **pseudo-exponential** map \f$ x \rightarrow
65  * PseudoExp(x^\wedge) \f$ and its Jacobian.
66  * - Input: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
67  * - Output: translation and rotation in SE(3) as CPose3D
68  * Note that this method implements retraction via a **pseudo-exponential**,
69  * where only the rotational part undergoes a real matrix exponential,
70  * while the translation is left unmodified. This is done for computational
71  * efficiency, and does not change the results of optimizations as long
72  * as the corresponding local coordinates (pseudo-logarithm) are used as
73  * well.
74  *
75  * See section 9.4.2 in \cite blanco_se3_tutorial
76  */
77  static type exp(const tangent_vector& x);
78 
79  /** SE(3) **pseudo-logarithm** map \f$ \mathbf{R} \rightarrow
80  * \log(\mathbf{R}^\vee)\f$
81  * - Input: translation and rotation in SE(3) as CPose3D
82  * - Output: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
83  *
84  * See exp() for the explanation about the "pseudo" name.
85  * For the formulas, see section 9.4.2 in \cite blanco_se3_tutorial
86  */
87  static tangent_vector log(const type& P);
88 
89  /** Returns a vector with all manifold matrix elements in column-major
90  * order. For SE(3), it is a 3x4=12 vector. */
91  static manifold_vector asManifoldVector(const type& pose);
92 
93  /** The inverse operation of asManifoldVector() */
94  static type fromManifoldVector(const manifold_vector& v);
95 
96  /** Jacobian for the pseudo-exponential exp().
97  * See 10.3.1 in \cite blanco_se3_tutorial
98  *
99  * - Input: 6-len vector in Lie algebra se(3)
100  * - Jacobian: Jacobian of the 3x4 matrix (stacked as a column major
101  * 12-vector) wrt the vector in the tangent space.
102  */
103  static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);
104 
105  /** Jacobian for the pseudo-logarithm log()
106  * See 10.3.11 in \cite blanco_se3_tutorial
107  *
108  * - Input: a SE(3) pose as a CPose3D object
109  * - Jacobian: Jacobian of the tangent space vector wrt the 3x4 matrix
110  * elements (stacked as a column major 12-vector).
111  */
112  static mat2tang_jacob jacob_dlogv_dv(const type& P);
113 
114  /** Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
115  * \note Section 10.3.3 in \cite blanco_se3_tutorial
116  */
117  static tang2mat_jacob jacob_dexpeD_de(const CPose3D& D);
118 
119  /** Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.
120  * \note Section 10.3.4 in \cite blanco_se3_tutorial
121  */
122  static tang2mat_jacob jacob_dDexpe_de(const CPose3D& D);
123 
124  /** Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie
125  * Algebra.
126  * \note Eq. 10.3.7 in \cite blanco_se3_tutorial
127  */
128  static tang2mat_jacob jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D);
129 
130  /** Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices,
131  * with respect to A.
132  * \note See section 7.3.1 of in \cite blanco_se3_tutorial
133  */
134  static matrix_MxM jacob_dAB_dA(const type& A, const type& B);
135 
136  /** Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices,
137  * with respect to B.
138  * \note See section 7.3.1 of in \cite blanco_se3_tutorial
139  */
140  static matrix_MxM jacob_dAB_dB(const type& A, const type& B);
141 
142  /** Return one or both of the following 6x6 Jacobians, useful in
143  * graph-slam problems:
144  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
145  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\f]
146  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ increments in the
147  * linearized manifold for P1 and P2.
148  * \note Section 10.3.10 in \cite blanco_se3_tutorial
149  */
150  static void jacob_dDinvP1invP2_de1e2(
151  const type& Dinv, const type& P1, const type& P2,
152  mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
153  mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt);
154 };
155 
156 /** Traits for SE(2), rigid-body transformations in R^2 space.
157  * See indidual members for documentation, or \cite blanco_se3_tutorial for a
158  * general overview.
159  * \ingroup mrpt_poses_lie_grp
160  */
161 template <>
162 struct SE<2>
163 {
164  /** Number of actual degrees of freedom for this transformation */
165  constexpr static size_t DOFs = 3;
166 
167  /** Dimensionality of the matrix manifold; this should be 3x3=9 for SE(2),
168  * but for efficiency, we define it as simply 3x1=3 and use the (x,y,phi)
169  * representation as well as the "manifold". This is done for API
170  * consistency with SE(3), where the actual matrix is used instead. */
171  constexpr static size_t MANIFOLD_DIM = 3;
172 
175 
176  using type = CPose2D;
178 
179  /** Type for Jacobians between SO(n) transformations */
181 
182  /** In SE(3), this type represents Jacobians between SO(n) (sub)matrices in
183  * the manifold, but in SE(2) it simply models Jacobians between (x,y,phi)
184  * vectors. */
186 
187  /** Type for Jacobian: tangent space -> SO(n) matrix */
189  /** Type for Jacobian: SO(n) matrix -> tangent space */
191 
192  /** Exponential map in SE(2), takes [x,y,phi] and returns a CPose2D */
193  static type exp(const tangent_vector& x);
194 
195  /** Logarithm map in SE(2), takes a CPose2D and returns [X,Y, phi] */
196  static tangent_vector log(const type& P);
197 
198  /** Returns a vector with all manifold matrix elements in column-major
199  * order. For SE(2), though, it directly returns the vector [x,y,phi] for
200  * efficiency in comparison to 3x3 homogeneous coordinates. */
201  static manifold_vector asManifoldVector(const type& pose);
202 
203  /** The inverse operation of asManifoldVector() */
204  static type fromManifoldVector(const manifold_vector& v);
205 
206  /** Jacobian of the pose composition A*B for SE(2) with respect to A.
207  * \note See appendix B of \cite blanco_se3_tutorial
208  */
209  static matrix_MxM jacob_dAB_dA(const type& A, const type& B);
210 
211  /** Jacobian of the pose composition A*B for SE(2) with respect to B.
212  * \note See appendix B of \cite blanco_se3_tutorial
213  */
214  static matrix_MxM jacob_dAB_dB(const type& A, const type& B);
215 
216  /** Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.
217  * \note See appendix B in \cite blanco_se3_tutorial
218  */
219  static tang2mat_jacob jacob_dDexpe_de(const type& D);
220 
221  /** Return one or both of the following 3x3 Jacobians, useful in
222  * graph-slam problems:
223  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
224  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\f]
225  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ increments in the
226  * linearized manifold for P1 and P2.
227  * \note See appendix B in \cite blanco_se3_tutorial
228  */
229  static void jacob_dDinvP1invP2_de1e2(
230  const type& Dinv, const type& P1, const type& P2,
231  mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
232  mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt);
233 };
234 
235 } // namespace mrpt::poses::Lie
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
CMatrixFixed< double, 12, 6 > CMatrixDouble12_6
Definition: CMatrixFixed.h:386
CMatrixFixed< double, 6, 12 > CMatrixDouble6_12
Definition: CMatrixFixed.h:385
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:369



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020