Gazebo Math
API Reference
7.4.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
eigen3
include
gz
math
eigen3
gz/math/eigen3/Conversions.hh
Go to the documentation of this file.
1
/*
2
* Copyright (C) 2018 Open Source Robotics Foundation
3
*
4
* Licensed under the Apache License, Version 2.0 (the "License");
5
* you may not use this file except in compliance with the License.
6
* You may obtain a copy of the License at
7
*
8
* http://www.apache.org/licenses/LICENSE-2.0
9
*
10
* Unless required by applicable law or agreed to in writing, software
11
* distributed under the License is distributed on an "AS IS" BASIS,
12
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13
* See the License for the specific language governing permissions and
14
* limitations under the License.
15
*
16
*/
17
18
#ifndef GZ_MATH_EIGEN3_CONVERSIONS_HH_
19
#define GZ_MATH_EIGEN3_CONVERSIONS_HH_
20
21
#include <Eigen/Geometry>
22
#include <
gz/math/AxisAlignedBox.hh
>
23
#include <
gz/math/Matrix3.hh
>
24
#include <
gz/math/Matrix6.hh
>
25
#include <
gz/math/Pose3.hh
>
26
#include <
gz/math/Quaternion.hh
>
27
#include <
gz/math/Vector3.hh
>
28
29
namespace
gz
30
{
31
namespace
math
32
{
33
namespace
eigen3
34
{
38
inline
Eigen::Vector3d
convert
(
const
gz::math::Vector3d
&_v)
39
{
40
return
Eigen::Vector3d(_v[0], _v[1], _v[2]);
41
}
42
47
inline
Eigen::AlignedBox3d
convert
(
48
const
gz::math::AxisAlignedBox
&_b)
49
{
50
return
Eigen::AlignedBox3d(
convert
(_b.
Min
()),
convert
(_b.
Max
()));
51
}
52
56
inline
Eigen::Matrix3d
convert
(
const
gz::math::Matrix3d
&_m)
57
{
58
Eigen::Matrix3d matrix;
59
for
(
std::size_t
i=0; i < 3; ++i)
60
{
61
for
(
std::size_t
j=0; j < 3; ++j)
62
{
63
matrix(i, j) = _m(i, j);
64
}
65
}
66
67
return
matrix;
68
}
69
75
template
<
typename
Precision>
76
inline
77
Eigen::Matrix<Precision, 6, 6>
convert
(
const
Matrix6<Precision>
&_m)
78
{
79
Eigen::Matrix<Precision, 6, 6> matrix;
80
for
(
std::size_t
i = 0; i < 6; ++i)
81
{
82
for
(
std::size_t
j = 0; j < 6; ++j)
83
{
84
matrix(i, j) = _m(i, j);
85
}
86
}
87
88
return
matrix;
89
}
90
94
inline
Eigen::Quaterniond
convert
(
const
gz::math::Quaterniond
&_q)
95
{
96
Eigen::Quaterniond quat;
97
quat.w() = _q.
W
();
98
quat.x() = _q.
X
();
99
quat.y() = _q.
Y
();
100
quat.z() = _q.
Z
();
101
102
return
quat;
103
}
104
108
inline
Eigen::Isometry3d
convert
(
const
gz::math::Pose3d
&_pose)
109
{
110
Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
111
tf.translation() =
convert
(_pose.
Pos
());
112
tf.linear() = Eigen::Matrix3d(
convert
(_pose.
Rot
()));
113
114
return
tf;
115
}
116
120
inline
gz::math::Vector3d
convert
(
const
Eigen::Vector3d &_v)
121
{
122
gz::math::Vector3d
vec;
123
vec.
X
() = _v[0];
124
vec.
Y
() = _v[1];
125
vec.
Z
() = _v[2];
126
127
return
vec;
128
}
129
133
inline
gz::math::AxisAlignedBox
convert
(
134
const
Eigen::AlignedBox3d &_b)
135
{
136
gz::math::AxisAlignedBox
box;
137
box.
Min
() =
convert
(_b.min());
138
box.
Max
() =
convert
(_b.max());
139
140
return
box;
141
}
142
146
inline
gz::math::Matrix3d
convert
(
const
Eigen::Matrix3d &_m)
147
{
148
gz::math::Matrix3d
matrix;
149
for
(
std::size_t
i=0; i < 3; ++i)
150
{
151
for
(
std::size_t
j=0; j < 3; ++j)
152
{
153
matrix(i, j) = _m(i, j);
154
}
155
}
156
157
return
matrix;
158
}
159
165
template
<
typename
Precision>
166
inline
167
Matrix6<Precision>
convert
(
const
Eigen::Matrix<Precision, 6, 6> &_m)
168
{
169
Matrix6<Precision>
matrix;
170
for
(
std::size_t
i = 0; i < 6; ++i)
171
{
172
for
(
std::size_t
j = 0; j < 6; ++j)
173
{
174
matrix(i, j) = _m(i, j);
175
}
176
}
177
178
return
matrix;
179
}
180
184
inline
gz::math::Quaterniond
convert
(
const
Eigen::Quaterniond &_q)
185
{
186
gz::math::Quaterniond
quat;
187
quat.
W
() = _q.w();
188
quat.
X
() = _q.x();
189
quat.
Y
() = _q.y();
190
quat.
Z
() = _q.z();
191
192
return
quat;
193
}
194
198
inline
gz::math::Pose3d
convert
(
const
Eigen::Isometry3d &_tf)
199
{
200
gz::math::Pose3d
pose;
201
pose.
Pos
() =
convert
(Eigen::Vector3d(_tf.translation()));
202
pose.
Rot
() =
convert
(Eigen::Quaterniond(_tf.linear()));
203
204
return
pose;
205
}
206
}
207
}
208
}
209
210
#endif