0. 唠叨几句
项目过程中经常要用到四元数,欧拉角还有旋转矩阵,所以它们之间的相互转化代码就经常会被调用,整理一下,以后就不用东找西找了。
在C++的版本中,我一般都是用Eigen库里面提供的API来完成转换的,所以需要先安装好Eigen库,如果没装的,可以按照我另一篇博客去安装,点击这里
在python版本中,可以自己写公式来算,基本用的都是numpy,这个就比较容易安装,不多说了。
也可以直接调用scipy库,用pip install scipy
安装
PS:
1)四元素,欧拉角和旋转矩阵之间的转换其实都有公式,用公式自己写个函数也是其中一种方法,我放在python版本处,C++调用现成的库会更方便
2)注意:所有的欧拉角都是按Z, Y, X的顺序旋转的
1. 四元数 -> 欧拉角
1.1 四元数 -> 欧拉角(C++)
#include <tf/tf.h>
#include <nav_msgs/Odometry.h>
double base_roll, base_pitch, base_yaw;
nav_msgs::Odometry odom_pose;
tf::Quaternion RQ2; //quaternion to RPY
tf::quaternionMsgToTF(odom_msg.pose.pose.orientation,RQ2);
tf::Matrix3x3(RQ2).getRPY(base_roll,base_pitch,base_yaw);
1.2 四元数 -> 欧拉角(Python)
方法一:用tf库
import tf
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
方法二:根据公式自己写函数
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
2. 四元数 -> 旋转矩阵
2.1 四元数 -> 旋转矩阵(C++)
#include <Eigen/Core>
#include <Eigen/Geometry>
Eigen::Quaterniond t_Q;
t_Q.x() = odom_pose.pose.pose.orientation.x;
t_Q.y() = odom_pose.pose.pose.orientation.y;
t_Q.z() = odom_pose.pose.pose.orientation.z;
t_Q.w() = odom_pose.pose.pose.orientation.w;
t_Q.normalize();
Eigen::Matrix3d R3;
R3 = t_Q.matrix();
cout << "R3: " << endl << R3 << endl;
2.2 四元数 -> 旋转矩阵(Python)
注意一下四元数的顺序就行,按xyzw来写
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])]]
)
return rot_matrix
Rq = [odom_pose.pose.pose.orientation.x, odom_pose.pose.pose.orientation.y, odom_pose.pose.pose.orientation.z, odom_pose.pose.pose.orientation.w]
R3 = quaternion_to_rotation_matrix(Rq)
3. 欧拉角 - > 四元数
3.1 欧拉角 -> 四元数(C++)
#include <Eigen/Core>
#include <Eigen/Geometry>
Eigen::Quaterniond quaternion3;
double roll, pitch, yaw;
quaternion3 = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
3.2 欧拉角 -> 四元数(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
4. 欧拉角 - > 旋转矩阵
4.1 欧拉角 -> 旋转矩阵(C++)
#include <Eigen/Core>
#include <Eigen/Geometry>
Eigen::Matrix3d rotation_matrix3;
rotation_matrix3 = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
4.2 欧拉角 -> 旋转矩阵(Python)
公式: r o t a t i o n m a t r i x = R z ∗ R y ∗ R x rotation_matrix = R_z * R_y * R_x rotationmatrix=Rz∗Ry∗Rx
import numpy as np
import math
def eul2rot(roll, pitch,yaw):
R_z = np.array([[math.cos(yaw), -math.sin(yaw), 0],
[math.sin(yaw), math.cos(yaw), 0],
[0, 0, 1]
])
R_y = np.array([[math.cos(pitch), 0, math.sin(pitch) ],
[0, 1, 0 ],
[-math.sin(pitch), 0, math.cos(pitch) ]
])
R_x = np.array([[1, 0, 0 ],
[0, math.cos(roll), -math.sin(roll) ],
[0, math.sin(roll), math.cos(roll) ]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
return R
5. 旋转矩阵 -> 四元数
5.1 旋转矩阵 -> 四元数(C++)
Eigen::Matrix3d R;
Eigen::Quaterniond q = Eigen::Quaterniond(R);
q.normalize();
cout << “Quaternion:” <<endl;
cout << ”x = ” << q.x() <<endl;
cout << ”y = ” << q.y() <<endl;
cout << ”z = ” << q.z() <<endl;
cout << ”w = ” << q.w() <<endl<<endl;
5.2 旋转矩阵 -> 四元数(Python)
方法1: 调用scipy库
import numpy as np
import math
from scipy.spatial.transform import Rotation as R
r3 = R.from_matrix(Rm)
qua = r3.as_quat()
方法2: 直接写公式
对于旋转矩阵到四元数的公式,可以看看引用里的第三篇blog,写得很详细
import numpy as np
import math
def rmat2Quat(R):
helper = np.array(np.abs([R[0][0]+R[1][1]+R[2][2],R[0][0]-R[1][1]-R[2][2],-R[0][0]+R[1][1]-R[2][2],-R[0][0]-R[1][1]+R[2][2]]))
pos = np.argmax(helper)
if(pos == 0):
w = np.sqrt(R[0][0]+R[1][1]+R[2][2]+1)/2
x = (R[1][2]-R[2][1])/4/w
y = (R[2][0]-R[0][2])/4/w
z = (R[0][1]-R[1][0])/4/w
elif(pos == 1):
x = np.sqrt(R[0][0]-R[1][1]-R[2][2]+1)/2
w = (R[1][2]-R[2][1])/4/x
y = (R[0][1]+R[1][0])/4/x
z = (R[2][0]+R[0][2])/4/x
elif(pos == 2):
y = np.sqrt(-R[0][0]+R[1][1]-R[2][2]+1)/2
w = (R[2][0]-R[0][2])/4/y
x = (R[0][1]+R[1][0])/4/y
z = (R[1][2]+R[2][1])/4/y
elif(pos == 3):
z = np.sqrt(-R[0][0]-R[1][1]+R[2][2]+1)/2
w = (R[0][1]-R[1][0])/4/z
x = (R[2][0]+R[0][2])/4/z
y = (R[1][2]+R[2][1])/4/z
return w,x,y,z
6. 旋转矩阵 -> 欧拉角
6.1 旋转矩阵 -> 欧拉角(C++)
#include <Eigen/Core>
#include <Eigen/Geometry>
Eigen::Matrix3d R3 = Eigen::Matrix3d::Identity();;
double roll, pitch, yaw;
//ZYX顺序,即先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,0表示X轴,1表示Y轴,2表示Z轴
Eigen::Vector3d euler_angles_3 = R3.eulerAngles(2, 1, 0);
6.2 旋转矩阵 -> 欧拉角(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])
7. 矩阵求逆
7.1 矩阵求逆(C++)
#include <Eigen/Core>
#include <Eigen/Geometry>
Eigen::Matrix3d trans_map2odom = Eigen::Matrix3d::Identity();
Eigen::Matrix3d trans_map2odom_inv = trans_map2odom.inverse();
7.2 矩阵求逆(Python)
求逆之前,最好先确认一下矩阵是否满稚,如果不满秩就直接报错了,不满秩的矩阵可以求伪逆,这个可以参考我的另一篇博客,点击这里
import numpy as np
trans_map2odom = np.mat([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype = np.float)
np.linalg.inv(trans_map2odom)