MRPT
2.0.3
mrpt
poses
Lie
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;
30
using
tangent_vector
=
mrpt::math::CVectorFixedDouble<DOFs>
;
31
using
type
=
mrpt::math::CMatrixDouble33
;
32
33
/** Type for Jacobian: tangent space -> SO(n) matrix */
34
using
tang2mat_jacob
=
mrpt::math::CMatrixDouble93
;
35
36
/** Type for Jacobian: SO(n) matrix -> tangent space */
37
using
mat2tang_jacob
=
mrpt::math::CMatrixDouble39
;
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;
91
using
tangent_vector
=
mrpt::math::CVectorFixedDouble<DOFs>
;
92
using
type
= double;
93
94
/** Type for Jacobian: tangent space to SO(n) matrix */
95
using
tang2mat_jacob
=
mrpt::math::CMatrixFixed<double, 1, 1>
;
96
/** Type for Jacobian: SO(n) matrix to tangent space */
97
using
mat2tang_jacob
=
mrpt::math::CMatrixFixed<double, 1, 1>
;
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
mrpt::poses::Lie::SO< 2 >::type
double type
Definition:
SO.h:92
CMatrixFixed.h
mrpt::obs::gnss::roll
double roll
Definition:
gnss_messages_novatel.h:306
R
const float R
Definition:
CKinematicChain.cpp:137
optional_ref.h
mrpt::poses::Lie::SO
Traits for SO(n), rotations in R^n space.
Definition:
SO.h:21
mrpt::poses::Lie
Definition:
Euclidean.h:16
CVectorFixed.h
mrpt::math::CMatrixDouble39
CMatrixFixed< double, 3, 9 > CMatrixDouble39
Definition:
CMatrixFixed.h:387
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition:
CMatrixFixed.h:33
mrpt::obs::gnss::pitch
double pitch
Definition:
gnss_messages_novatel.h:306
mrpt::math::CMatrixDouble93
CMatrixFixed< double, 9, 3 > CMatrixDouble93
Definition:
CMatrixFixed.h:388
mrpt::math::CMatrixDouble33
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition:
CMatrixFixed.h:367
Page generated by
Doxygen 1.8.17
for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020