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

在摄影测量学科中，国际摄影测量遵循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,0)).toRotationMatrix();  Eigen::Matrix3d R_y=Eigen::AngleAxisd(theta(1),Eigen::Vector3d(0,1,0)).toRotationMatrix();  Eigen::Matrix3d R_z=Eigen::AngleAxisd(theta(2),Eigen::Vector3d(0,0,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,1,0).transpose()<<endl;//z-y-x顺序，与theta顺序是x,y,z  }  else{    assert(isRotationMatirx(R));  }  return 0;}

#!/usr/bin/env python3# -*- coding: utf-8 -*-import cv2import numpy as npimport mathimport randomdef 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-6def 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])def eulerAnglesToRotationMatrix(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 ))  return Rif __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))