python 和c++实现旋转矩阵到欧拉角的变换方式
在摄影测量学科中,国际摄影测量遵循OPK系统,即是xyz转角系统,而工业中往往使用zyx转角系统。
旋转矩阵的意义:描述相对地面的旋转情况,yaw-pitch-roll对应zyx对应k,p,w
#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(发邮件时,请将#更换为@)进行举报,并提供相关证据,一经查实,本站将立刻删除涉嫌侵权内容。