ftf_quaternion_utils.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2015,2016 Vladimir Ermakov.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 #include <mavros/frame_tf.h>
17 
18 namespace mavros {
19 namespace ftf {
20 
21 /*
22  * Note: order of axis are match tf2::LinearMath (bullet).
23  * YPR rotation convention -> YAW first, Pitch second, Roll third
24  * Compatibility checked by unittests.
25  */
26 
27 Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
28 {
29  // YPR - ZYX
30  return Eigen::Quaterniond(
31  Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) *
32  Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
33  Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX())
34  );
35 }
36 
37 Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q)
38 {
39  // YPR - ZYX
40  return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
41 }
42 
43 double quaternion_get_yaw(const Eigen::Quaterniond &q)
44 {
45  // to match equation from:
46  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
47  const double &q0 = q.w();
48  const double &q1 = q.x();
49  const double &q2 = q.y();
50  const double &q3 = q.z();
51 
52  return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
53 }
54 
55 } // namesapace ftf
56 } // namesapace mavros
Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q)
Convert quaternion to euler angles.
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Convert euler angles to quaternion.
double quaternion_get_yaw(const Eigen::Quaterniond &q)
Get Yaw angle from quaternion.
Frame transformation utilities.


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26