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

在摄影测量学科中，国际摄影测量遵循OPK系统，即是xyz转角系统，而工业中往往使用zyx转角系统。

旋转矩阵的意义：描述相对地面的旋转情况，yaw-pitch-roll对应zyx对应k,p,w

#include

#include

#include

#include

#include

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

}

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<

R=rotationVectorToMatrix(theta);

cout<

if(! isRotationMatirx(R)){

cout<欧拉角\n"<

}

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 ("\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))