python旋转矩阵_欧拉⾓，四元数，旋转矩阵相互转化
（c++,python）
四元数->欧拉⾓
C++
#include <tf/tf.h>
tf::Quaternion quat;
tf::quaternionMsgToTF(odom.ientation, quat);
double roll, pitch, yaw;//定义存储rpy的容器
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([ientation.x, ientation.y, ientation.z, ientation.w])
欧拉⾓->四元数
headcounts
C++
#include <tf/tf.h>
tf::Quaternion q;
q.tRPY(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<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBa<Derived> &ypr) {
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<Scalar_t, 3, 3> Rz;
Rz << cos(y), -sin(y), 0,
sin(y), cos(y), 0,
0, 0, 1;
Eigen::Matrix<Scalar_t, 3, 3> Ry;
Ry << cos(p), 0., sin(p),
0., 1., 0.,
-sin(p), 0., cos(p);
Eigen::Matrix<Scalar_t, 3, 3> Rx;
高考补习班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, s(theta[0]), -math.sin(theta[0]) ],
[0, math.sin(theta[0]), s(theta[0]) ]
])
R_y = np.array([[s(theta[1]), 0, math.sin(theta[1]) ],
[0, 1, 0 ],
[-math.sin(theta[1]), 0, s(theta[1]) ]
])
R_z = np.array([[s(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), s(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))galvanized steel
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);
undergrounddouble y = atan2(n(1), n(0));
smile意思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) :
asrt(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])
el :
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);
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));
el
% 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 ;artistry
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));
elif 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));
elif 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));
elif 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));
contact是什么意思
end
四元数->旋转矩阵
四元数
brainiac
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__(lf):
lf.x = 0
lf.y = 0
lf.z = 0
lf.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
habefrom ansform 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的旋转⾓度计算四元数，⽤于平⾯⼩车。返回四元数
