Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gepetto
aicp_mapping
Commits
92d17c3e
Commit
92d17c3e
authored
Apr 24, 2019
by
Maurice Fallon
Browse files
add conversion
parent
dd3fd5c3
Changes
2
Hide whitespace changes
Inline
Side-by-side
aicp_core/include/aicp_utils/common.hpp
View file @
92d17c3e
...
...
@@ -49,3 +49,4 @@ Eigen::VectorXf get_random_gaussian_variable(float mean, float std_deviation, in
void
quat_to_euler
(
Eigen
::
Quaterniond
q
,
double
&
roll
,
double
&
pitch
,
double
&
yaw
);
Eigen
::
Quaterniond
euler_to_quat
(
double
roll
,
double
pitch
,
double
yaw
);
aicp_core/src/utils/common.cpp
View file @
92d17c3e
...
...
@@ -76,3 +76,30 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya
pitch
=
asin
(
2
*
(
q0
*
q2
-
q3
*
q1
));
yaw
=
atan2
(
2
*
(
q0
*
q3
+
q1
*
q2
),
1
-
2
*
(
q2
*
q2
+
q3
*
q3
));
}
Eigen
::
Quaterniond
euler_to_quat
(
double
roll
,
double
pitch
,
double
yaw
)
{
// This conversion function introduces a NaN in Eigen Rotations when:
// roll == pi , pitch,yaw =0 ... or other combinations.
// cos(pi) ~=0 but not exactly 0
if
(
((
roll
==
M_PI
)
&&
(
pitch
==
0
))
&&
(
yaw
==
0
)){
return
Eigen
::
Quaterniond
(
0
,
1
,
0
,
0
);
}
else
if
(
((
pitch
==
M_PI
)
&&
(
roll
==
0
))
&&
(
yaw
==
0
)){
return
Eigen
::
Quaterniond
(
0
,
0
,
1
,
0
);
}
else
if
(
((
yaw
==
M_PI
)
&&
(
roll
==
0
))
&&
(
pitch
==
0
)){
return
Eigen
::
Quaterniond
(
0
,
0
,
0
,
1
);
}
double
sy
=
sin
(
yaw
*
0.5
);
double
cy
=
cos
(
yaw
*
0.5
);
double
sp
=
sin
(
pitch
*
0.5
);
double
cp
=
cos
(
pitch
*
0.5
);
double
sr
=
sin
(
roll
*
0.5
);
double
cr
=
cos
(
roll
*
0.5
);
double
w
=
cr
*
cp
*
cy
+
sr
*
sp
*
sy
;
double
x
=
sr
*
cp
*
cy
-
cr
*
sp
*
sy
;
double
y
=
cr
*
sp
*
cy
+
sr
*
cp
*
sy
;
double
z
=
cr
*
cp
*
sy
-
sr
*
sp
*
cy
;
return
Eigen
::
Quaterniond
(
w
,
x
,
y
,
z
);
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment