python 和c++實現旋轉矩陣到尤拉角的變換方式
阿新 • • 發佈:2020-01-09
在攝影測量學科中,國際攝影測量遵循OPK系統,即是xyz轉角系統,而工業中往往使用zyx轉角系統。
旋轉矩陣的意義:描述相對地面的旋轉情況,yaw-pitch-roll對應zyx對應k,p,w
#include <iostream> #include<stdlib.h> #include<eigen3/Eigen/Core> #include<eigen3/Eigen/Dense> #include<stdlib.h> using namespace std; Eigen::Matrix3d rotationVectorToMatrix(Eigen::Vector3d theta) { Eigen::Matrix3d R_x=Eigen::AngleAxisd(theta(0),Eigen::Vector3d(1,0)).toRotationMatrix(); Eigen::Matrix3d R_y=Eigen::AngleAxisd(theta(1),Eigen::Vector3d(0,1,0)).toRotationMatrix(); Eigen::Matrix3d R_z=Eigen::AngleAxisd(theta(2),1)).toRotationMatrix(); return R_z*R_y*R_x; } bool isRotationMatirx(Eigen::Matrix3d R) { int err=1e-6;//判斷R是否奇異 Eigen::Matrix3d shouldIdenity; shouldIdenity=R*R.transpose(); Eigen::Matrix3d I=Eigen::Matrix3d::Identity(); return (shouldIdenity-I).norm()<err?true:false; } int main(int argc,char *argv[]) { Eigen::Matrix3d R; Eigen::Vector3d theta(rand() % 360 - 180.0,rand() % 360 - 180.0,rand() % 360 - 180.0); theta=theta*M_PI/180; cout<<"旋轉向量是:\n"<<theta.transpose()<<endl; R=rotationVectorToMatrix(theta); cout<<"旋轉矩陣是:\n"<<R<<endl; if(! isRotationMatirx(R)){ cout<<"旋轉矩陣--->尤拉角\n"<<R.eulerAngles(2,0).transpose()<<endl;//z-y-x順序,與theta順序是x,y,z } else{ assert(isRotationMatirx(R)); } return 0; }
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import cv2 import numpy as np import math import random def isRotationMatrix(R) : Rt = np.transpose(R) shouldBeIdentity = np.dot(Rt,R) I = np.identity(3,dtype = R.dtype) n = np.linalg.norm(I - shouldBeIdentity) return n < 1e-6 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,R[0,0]) else : x = math.atan2(-R[1,2],R[1,1]) y = math.atan2(-R[2,sy) z = 0 return np.array([x,z]) def eulerAnglesToRotationMatrix(theta) : R_x = np.array([[1,0 ],[0,math.cos(theta[0]),-math.sin(theta[0]) ],math.sin(theta[0]),math.cos(theta[0]) ] ]) R_y = np.array([[math.cos(theta[1]),math.sin(theta[1]) ],[-math.sin(theta[1]),math.cos(theta[1]) ] ]) R_z = np.array([[math.cos(theta[2]),-math.sin(theta[2]),[math.sin(theta[2]),math.cos(theta[2]),1] ]) R = np.dot(R_z,np.dot( R_y,R_x )) return R if __name__ == '__main__' : e = np.random.rand(3) * math.pi * 2 - math.pi R = eulerAnglesToRotationMatrix(e) e1 = rotationMatrixToEulerAngles(R) R1 = eulerAnglesToRotationMatrix(e1) print ("\nInput Euler angles :\n{0}".format(e)) print ("\nR :\n{0}".format(R)) print ("\nOutput Euler angles :\n{0}".format(e1)) print ("\nR1 :\n{0}".format(R1))
以上這篇python 和c++實現旋轉矩陣到尤拉角的變換方式就是小編分享給大家的全部內容了,希望能給大家一個參考,也希望大家多多支援我們。