python 和c++实现旋转矩阵到欧拉角的变换方式

2023-08-05 01:09:05 87

#include #include #include #include #include usingnamespacestd; Eigen::Matrix3drotationVectorToMatrix(Eigen::Vector3dtheta) { Eigen::Matrix3dR_x=Eigen::AngleAxisd(theta(0),Eigen::Vector3d(1,0,0)).toRotationMatrix(); Eigen::Matrix3dR_y=Eigen::AngleAxisd(theta(1),Eigen::Vector3d(0,1,0)).toRotationMatrix(); Eigen::Matrix3dR_z=Eigen::AngleAxisd(theta(2),Eigen::Vector3d(0,0,1)).toRotationMatrix(); returnR_z*R_y*R_x; } boolisRotationMatirx(Eigen::Matrix3dR) { interr=1e-6;//判断R是否奇异 Eigen::Matrix3dshouldIdenity; shouldIdenity=R*R.transpose(); Eigen::Matrix3dI=Eigen::Matrix3d::Identity(); return(shouldIdenity-I).norm()欧拉角\n"<

#!/usr/bin/envpython3
#-*-coding:utf-8-*-

importcv2
importnumpyasnp
importmath
importrandom

defisRotationMatrix(R):
Rt=np.transpose(R)
shouldBeIdentity=np.dot(Rt,R)
I=np.identity(3,dtype=R.dtype)
n=np.linalg.norm(I-shouldBeIdentity)
returnn<1e-6

defrotationMatrixToEulerAngles(R):

assert(isRotationMatrix(R))

sy=math.sqrt(R[0,0]*R[0,0]+R[1,0]*R[1,0])

singular=sy<1e-6

ifnotsingular:
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

returnnp.array([x,y,z])

defeulerAnglesToRotationMatrix(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))

returnR


if__name__=='__main__':

e=np.random.rand(3)*math.pi*2-math.pi

R=eulerAnglesToRotationMatrix(e)
e1=rotationMatrixToEulerAngles(R)

R1=eulerAnglesToRotationMatrix(e1)
print("\nInputEulerangles:\n{0}".format(e))
print("\nR:\n{0}".format(R))
print("\nOutputEulerangles:\n{0}".format(e1))
print("\nR1:\n{0}".format(R1))

以上这篇python和c++实现旋转矩阵到欧拉角的变换方式就是小编分享给大家的全部内容了，希望能给大家一个参考，也希望大家多多支持毛票票。

声明：本文内容来源于网络，版权归原作者所有，内容由互联网用户自发贡献自行上传，本网站不拥有所有权，未作人工编辑处理，也不承担相关法律责任。如果您发现有涉嫌版权的内容，欢迎发送邮件至：czq8825#qq.com（发邮件时，请将#更换为@）进行举报，并提供相关证据，一经查实，本站将立刻删除涉嫌侵权内容。

python 和c++实现旋转矩阵到欧拉角的变换方式

热门推荐

随机推荐