尤拉角，四元數，旋轉矩陣相互轉化（c++, python)
四元數->尤拉角
C++
#include
tf
：：
Quaternion
quat
；
tf
：：
quaternionMsgToTF
（
odom
。
pose
。
pose
。
orientation
，
quat
）；
double
roll
，
pitch
，
yaw
；
//定義儲存r\p\y的容器
tf
：：
Matrix3x3
（
quat
）。
getRPY
（
roll
，
pitch
，
yaw
）；
//進行轉換
PYTHON
def
quart_to_rpy
（
x
，
y
，
z
，
w
）
：
roll
=
math
。
atan2
（
2
*
（
w
*
x
+
y
*
z
），
1
-
2
*
（
x
*
x
+
y
*
y
））
pitch
=
math
。
asin
（
2
*
（
w
*
y
-
x
*
z
））
yaw
=
math
。
atan2
（
2
*
（
w
*
z
+
x
*
y
），
1
-
2
*
（
z
*
z
+
y
*
y
））
return
roll
，
pitch
，
yaw
# 使用 tf 庫
import
tf
（
r
，
p
，
y
）
=
tf
。
transformations
。
euler_from_quaternion
（［
msg
。
orientation
。
x
，
msg
。
orientation
。
y
，
msg
。
orientation
。
z
，
msg
。
orientation
。
w
］）
尤拉角->四元數
C++
#include
tf：：Quaternion q；
q。setRPY（roll， pitch， yaw）；
#create ros msg
tf：：Quaternion createQuaternionFromRPY（double roll，double pitch，double yaw）
PYTHON
def
rpy2quaternion
（
roll
，
pitch
，
yaw
）：
x
=
sin
（
pitch
/
2
）
sin
（
yaw
/
2
）
cos
（
roll
/
2
）
+
cos
（
pitch
/
2
）
cos
（
yaw
/
2
）
sin
（
roll
/
2
）
y
=
sin
（
pitch
/
2
）
cos
（
yaw
/
2
）
cos
（
roll
/
2
）
+
cos
（
pitch
/
2
）
sin
（
yaw
/
2
）
sin
（
roll
/
2
）
z
=
cos
（
pitch
/
2
）
sin
（
yaw
/
2
）
cos
（
roll
/
2
）
-
sin
（
pitch
/
2
）
cos
（
yaw
/
2
）
sin
（
roll
/
2
）
w
=
cos
（
pitch
/
2
）
cos
（
yaw
/
2
）
cos
（
roll
/
2
）
-
sin
（
pitch
/
2
）
sin
（
yaw
/
2
）
sin
（
roll
/
2
）
return
x
，
y
，
z
，
w
尤拉角->旋轉矩陣
C++
static Eigen：：Matrix
{
typedef typename Derived：：Scalar Scalar_t；
Scalar_t y = ypr（0） / 180。0 * M_PI；
Scalar_t p = ypr（1） / 180。0 * M_PI；
Scalar_t r = ypr（2） / 180。0 * M_PI；
Eigen：：Matrix
Rz << cos（y）， -sin（y）， 0，
sin（y）， cos（y）， 0，
0， 0， 1；
Eigen：：Matrix
Ry << cos（p）， 0。， sin（p），
0。， 1。， 0。，
-sin（p）， 0。， cos（p）；
Eigen：：Matrix
Rx << 1。， 0。， 0。，
0。， cos（r）， -sin（r），
0。， sin（r）， cos（r）；
return Rz * Ry * Rx；
}
PYTHON
def
eulerAnglesToRotationMatrix
（
theta
）
：
R_x
=
np
。
array
（［［
1
，
0
，
0
］，
［
0
，
math
。
cos
（
theta
［
0
］），
-
math
。
sin
（
theta
［
0
］）
］，
［
0
，
math
。
sin
（
theta
［
0
］），
math
。
cos
（
theta
［
0
］）
］
］）
R_y
=
np
。
array
（［［
math
。
cos
（
theta
［
1
］），
0
，
math
。
sin
（
theta
［
1
］）
］，
［
0
，
1
，
0
］，
［
-
math
。
sin
（
theta
［
1
］），
0
，
math
。
cos
（
theta
［
1
］）
］
］）
R_z
=
np
。
array
（［［
math
。
cos
（
theta
［
2
］），
-
math
。
sin
（
theta
［
2
］），
0
］，
［
math
。
sin
（
theta
［
2
］），
math
。
cos
（
theta
［
2
］），
0
］，
［
0
，
0
，
1
］
］）
R
=
np
。
dot
（
R_z
，
np
。
dot
（
R_y
，
R_x
））
return
R
旋轉矩陣->尤拉角
則尤拉角為
C++
static
Eigen
：：
Vector3d
R2ypr
（
const
Eigen
：：
Matrix3d
&
R
）
{
Eigen
：：
Vector3d
n
=
R
。
col
（
0
）；
Eigen
：：
Vector3d
o
=
R
。
col
（
1
）；
Eigen
：：
Vector3d
a
=
R
。
col
（
2
）；
Eigen
：：
Vector3d
ypr
（
3
）；
double
y
=
atan2
（
n
（
1
），
n
（
0
））；
double
p
=
atan2
（
-
n
（
2
），
n
（
0
）
*
cos
（
y
）
+
n
（
1
）
*
sin
（
y
））；
double
r
=
atan2
（
a
（
0
）
*
sin
（
y
）
-
a
（
1
）
*
cos
（
y
），
-
o
（
0
）
*
sin
（
y
）
+
o
（
1
）
*
cos
（
y
））；
ypr
（
0
）
=
y
；
ypr
（
1
）
=
p
；
ypr
（
2
）
=
r
；
return
ypr
/
M_PI
*
180。0
；
}
PYTHON
def
rotationMatrixToEulerAngles
（
R
）
：
assert
（
isRotationMatrix
（
R
））
sy
=
math
。
sqrt
（
R
［
0
，
0
］
*
R
［
0
，
0
］
+
R
［
1
，
0
］
*
R
［
1
，
0
］）
singular
=
sy
<
1e-6
if
not
singular
：
x
=
math
。
atan2
（
R
［
2
，
1
］
，
R
［
2
，
2
］）
y
=
math
。
atan2
（
-
R
［
2
，
0
］，
sy
）
z
=
math
。
atan2
（
R
［
1
，
0
］，
R
［
0
，
0
］）
else
：
x
=
math
。
atan2
（
-
R
［
1
，
2
］，
R
［
1
，
1
］）
y
=
math
。
atan2
（
-
R
［
2
，
0
］，
sy
）
z
=
0
return
np
。
array
（［
x
，
y
，
z
］）
旋轉矩陣->四元數
則四元數為
C++
Eigen
：：
Quaterniond
rotationMatrix2Quaterniond
（
Eigen
：：
Matrix3d
R
）
{
Eigen
：：
Quaterniond
q
=
Eigen
：：
Quaterniond
（
R
）；
q
。
normalize
（）；
cout
<<
“
RotationMatrix2Quaterniond
result
is
：
”
<<
endl
；
cout
<<
”
x
=
”
<<
q
。
x
（）
<<
endl
；
cout
<<
”
y
=
”
<<
q
。
y
（）
<<
endl
；
cout
<<
”
z
=
”
<<
q
。
z
（）
<<
endl
；
cout
<<
”
w
=
”
<<
q
。
w
（）
<<
endl
<<
endl
；
return
q
；
}
PYTHON
function
q
=
vgg_quat_from_rotation_matrix
（
R
）
%
vgg_quat_from_rotation_matrix
Generates
quaternion
from
rotation
matrix
%
q
=
vgg_quat_from_rotation_matrix
（
R
）
q
=
［
（
1
+
R
（
1
，
1
）
+
R
（
2
，
2
）
+
R
（
3
，
3
））
（
1
+
R
（
1
，
1
）
-
R
（
2
，
2
）
-
R
（
3
，
3
））
（
1
-
R
（
1
，
1
）
+
R
（
2
，
2
）
-
R
（
3
，
3
））
（
1
-
R
（
1
，
1
）
-
R
（
2
，
2
）
+
R
（
3
，
3
））
］；
if
~
issym
（
q
）
%
Pivot
to
avoid
division
by
small
numbers
［
b
I
］
=
max
（
abs
（
q
））；
else
%
For
symbolic
quats
，
just
make
sure
we
‘re nonzero
for
k
=
1
：
4
if
q
（
k
）
~=
0
I
=
k
；
break
end
end
end
q
（
I
）
=
sqrt
（
q
（
I
））
/
2
；
if
I
==
1
q
（
2
）
=
（
R
（
3
，
2
）
-
R
（
2
，
3
））
/
（
4
*
q
（
I
））；
q
（
3
）
=
（
R
（
1
，
3
）
-
R
（
3
，
1
））
/
（
4
*
q
（
I
））；
q
（
4
）
=
（
R
（
2
，
1
）
-
R
（
1
，
2
））
/
（
4
*
q
（
I
））；
elseif
I
==
2
q
（
1
）
=
（
R
（
3
，
2
）
-
R
（
2
，
3
））
/
（
4
*
q
（
I
））；
q
（
3
）
=
（
R
（
2
，
1
）
+
R
（
1
，
2
））
/
（
4
*
q
（
I
））；
q
（
4
）
=
（
R
（
1
，
3
）
+
R
（
3
，
1
））
/
（
4
*
q
（
I
））；
elseif
I
==
3
q
（
1
）
=
（
R
（
1
，
3
）
-
R
（
3
，
1
））
/
（
4
*
q
（
I
））；
q
（
2
）
=
（
R
（
2
，
1
）
+
R
（
1
，
2
））
/
（
4
*
q
（
I
））；
q
（
4
）
=
（
R
（
3
，
2
）
+
R
（
2
，
3
））
/
（
4
*
q
（
I
））；
elseif
I
==
4
q
（
1
）
=
（
R
（
2
，
1
）
-
R
（
1
，
2
））
/
（
4
*
q
（
I
））；
q
（
2
）
=
（
R
（
1
，
3
）
+
R
（
3
，
1
））
/
（
4
*
q
（
I
））；
q
（
3
）
=
（
R
（
3
，
2
）
+
R
（
2
，
3
））
/
（
4
*
q
（
I
））；
end
四元數->旋轉矩陣
四元數
C++
Eigen
：：
Matrix3d
Quaternion2RotationMatrix
（
const
double
x
，
const
double
y
，
const
double
z
，
const
double
w
）
{
Eigen
：：
Quaterniond
q
；
q
。
x
（）
=
x
；
q
。
y
（）
=
y
；
q
。
z
（）
=
z
；
q
。
w
（）
=
w
；
Eigen
：：
Matrix3d
R
=
q
。
normalized
（）。
toRotationMatrix
（）；
cout
<<
“
Quaternion2RotationMatrix
result
is
：
”
<<
endl
；
cout
<<
”
R
=
”
<<
endl
<<
R
<<
endl
<<
endl
；
return
R
；
}
PYTHON
def
quaternion_to_rotation_matrix
（
q
）：
# x， y ，z ，w
rot_matrix
=
np
。
array
（
［［
1。0
-
2
*
（
q
［
1
］
*
q
［
1
］
+
q
［
2
］
*
q
［
2
］），
2
*
（
q
［
0
］
*
q
［
1
］
-
q
［
3
］
*
q
［
2
］），
2
*
（
q
［
3
］
*
q
［
1
］
+
q
［
0
］
*
q
［
2
］）］，
［
2
*
（
q
［
0
］
*
q
［
1
］
+
q
［
3
］
*
q
［
2
］），
1。0
-
2
*
（
q
［
0
］
*
q
［
0
］
+
q
［
2
］
*
q
［
2
］），
2
*
（
q
［
1
］
*
q
［
2
］
-
q
［
3
］
*
q
［
0
］）］，
［
2
*
（
q
［
0
］
*
q
［
2
］
-
q
［
3
］
*
q
［
1
］），
2
*
（
q
［
1
］
*
q
［
2
］
+
q
［
3
］
*
q
［
0
］），
1。0
-
2
*
（
q
［
0
］
*
q
［
0
］
+
q
［
1
］
*
q
［
1
］）］］，
dtype
=
q
。
dtype
）
return
rot_matrix
也可以用tf 的或者scipy的轉換
# tf
from tf。listener import xyzw_to_mat44
class oritation：
def __init__（self）：
self。x = 0
self。y = 0
self。z = 0
self。w = 0
ori = oritation（）
ori。x = rot［0］
ori。y = rot［1］
ori。z = rot［2］
ori。w = rot［3］
mat44 = xyzw_to_mat44（ori） # 轉換的結果為4 × 4 矩陣
# scipy
from scipy。spatial。transform import Rotation as R
# （x， y， z， w） format
r = R。from_quat（［-0。716556549511624，-0。6971278819736084， -0。010016582945017661， 0。02142651612120239］）
r。as_matrix（）
print（’rotation：\n‘，r。as_matrix（））
rotation_matrix = r。as_matrix（）
print（rotation_matrix）
其他
tf
：：
createQuaternionMsgFromYaw
（
double
y
）；
//只通過y即繞z的旋轉角度計算四元數，用於平面小車。返回四元數
