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

```#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<<"旋转向量是：
"<<theta.transpose()<<endl;
R=rotationVectorToMatrix(theta);
cout<<"旋转矩阵是：
"<<R<<endl;
if(! isRotationMatirx(R)){
cout<<"旋转矩阵--->欧拉角
"<<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 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,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 R

if __name__ == "__main__" :

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

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

R1 = eulerAnglesToRotationMatrix(e1)
print ("
Input Euler angles :
{0}".format(e))
print ("
R :
{0}".format(R))
print ("
Output Euler angles :
{0}".format(e1))
print ("
R1 :
{0}".format(R1))

```