相机欧拉角转旋转矩阵python 欧拉角转换旋转矩阵

欧拉角转旋转矩阵

对于两个三维点

 ，由点经过旋转矩阵旋转到，则有：

 任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。 对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现，如下图（蓝色是起始坐标系，而红色的是旋转之后的坐标系） ：

 

 

因此欧拉角转旋转矩阵如下：

则可以如下表示欧拉角：

以下代码用来实现旋转矩阵和欧拉角之间的相互变换。
C++语言：

Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
        1,       0,              0,
        0,       cos(theta[0]),   -sin(theta[0]),
        0,       sin(theta[0]),   cos(theta[0])
    );
    // Calculate rotation about y axis
    Mat R_y = (Mat_<double>(3,3) <<
        cos(theta[1]),    0,      sin(theta[1]),
        0,               1,      0,
        -sin(theta[1]),   0,      cos(theta[1])
    );
    // Calculate rotation about z axis
    Mat R_z = (Mat_<double>(3,3) <<
        cos(theta[2]),    -sin(theta[2]),      0,
        sin(theta[2]),    cos(theta[2]),       0,
        0,               0,                  1
    );
    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;
    return R;
}
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    #if 1
    x = x*180.0f/3.141592653589793f;
    y = y*180.0f/3.141592653589793f;
    z = z*180.0f/3.141592653589793f;
    #endif
    return Vec3f(x, y, z);
}

Python语言实现：

def rotationMatrixToEulerAngles(rvecs):
    R = np.zeros((3, 3), dtype=np.float64)
    cv2.Rodrigues(rvecs, 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
    #print('dst:', R)
    x = x*180.0/3.141592653589793
    y = y*180.0/3.141592653589793
    z = z*180.0/3.141592653589793
    return x,y,z
def eulerAnglesToRotationMatrix(angles1) :
    theta = np.zeros((3, 1), dtype=np.float64)
    theta[0] = angles1[0]*3.141592653589793/180.0
    theta[1] = angles1[1]*3.141592653589793/180.0
    theta[2] = angles1[2]*3.141592653589793/180.0
    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 ))
    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
    #print('dst:', R)
    x = x*180.0/3.141592653589793
    y = y*180.0/3.141592653589793
    z = z*180.0/3.141592653589793
    #rvecs = np.zeros((1, 1, 3), dtype=np.float64)
    #rvecs,_ = cv2.Rodrigues(R, rvecstmp)
    return x,y,z

旋转矩阵与旋转向量

处理三维旋转问题时，通常采用旋转矩阵的方式来描述。一个向量乘以旋转矩阵等价于向量以某种方式进行旋转。除了采用旋转矩阵描述外，还可以用旋转向量来描述旋转，旋转向量的长度（模）表示绕轴逆时针旋转的角度（弧度）。
旋转向量与旋转矩阵可以通过罗德里格斯（Rodrigues）变换进行转换。

其中：norm为求向量的模。
反变换也可以很容易的通过如下公式实现：

以下代码用来实现旋转向量到旋转矩阵及旋转矩阵到旋转向量的转换：

r_vec[0]=(double)(rotation_vectors[i](0));
        r_vec[1]=(double)(rotation_vectors[i](1));
        r_vec[2]=(double)(rotation_vectors[i](2));
        
        cvInitMatHeader(pr_vec,1,3,CV_64FC1,r_vec,CV_AUTOSTEP);
        cvInitMatHeader(pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);
        
        cvRodrigues2(pr_vec, pR_matrix, 0);//从旋转向量求旋转矩阵
        
        cvRodrigues2(pR_matrix, pnew_vec, 0);//从旋转矩阵求旋转向量
        Mat rotation_vec_tmp(pnew_vec->rows,pnew_vec->cols,pnew_vec->type,pnew_vec->data.fl);
        
        //cvMat转Mat
        Mat rotation_matrix_tmp(pR_matrix->rows,pR_matrix->cols,pR_matrix->type,pR_matrix->data.fl);
        //eulerAngles = rotationMatrixToEulerAngles(rotation_matrix_tmp, 0);
        //rotation_matrix_tmp = eulerAnglesToRotationMatrix(eulerAngles);
        eulerAngles = rotationMatrixToEulerAngles(rotation_matrix_tmp, 1);//0表示输出为弧度，否则表示输出为角度

对应的python代码如下：

R = np.zeros((3, 3), dtype=np.float64)
    cv2.Rodrigues(rvecs, R)
    rvecs = np.zeros((1, 1, 3), dtype=np.float64)
    rvecs,_ = cv2.Rodrigues(R, rvecstmp)

相互转换的完整代码

完整的测试C++程序如下：

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>
#include <cmath>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace cv;
using namespace std;

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
        1,       0,              0,
        0,       cos(theta[0]),   -sin(theta[0]),
        0,       sin(theta[0]),   cos(theta[0])
    );
    // Calculate rotation about y axis
    Mat R_y = (Mat_<double>(3,3) <<
        cos(theta[1]),    0,      sin(theta[1]),
        0,               1,      0,
        -sin(theta[1]),   0,      cos(theta[1])
    );
    // Calculate rotation about z axis
    Mat R_z = (Mat_<double>(3,3) <<
        cos(theta[2]),    -sin(theta[2]),      0,
        sin(theta[2]),    cos(theta[2]),       0,
        0,               0,                  1
    );
    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;
    return R;
}

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());
    return  norm(I, shouldBeIdentity) < 1e-6;
}
 
// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
    //assert(isRotationMatrix(R));
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    #if 1
    x = x*180.0f/3.141592653589793f;
    y = y*180.0f/3.141592653589793f;
    z = z*180.0f/3.141592653589793f;
    #endif
    return Vec3f(x, y, z);
}

int main()
{
    //Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
    Vec3f eulerAngles;
    
    int i;
    //double r_vec[3]={-2.100418,-2.167796,0.273330};[[0.78520514][0.0233998 ][0.00969251
    double r_vec[3]={0.78520514,0.0233998,0.00969251};//eulerAngles[45,1,1]
    double R_matrix[9];
    //CvMat *pr_vec;
    //CvMat *pR_matrix;
    CvMat *pr_vec = cvCreateMat(1,3,CV_64FC1);
    CvMat *pR_matrix = cvCreateMat(3,3,CV_64FC1);
    cvInitMatHeader(pr_vec, 1, 3, CV_64FC1, r_vec, CV_AUTOSTEP);
    cvInitMatHeader(pR_matrix, 3, 3, CV_64FC1, R_matrix, CV_AUTOSTEP);
    cvRodrigues2(pr_vec, pR_matrix, 0);
    Mat rotation_matrix(pR_matrix->rows,pR_matrix->cols,pR_matrix->type,pR_matrix->data.fl);
    eulerAngles = rotationMatrixToEulerAngles(rotation_matrix);
    cout << "pR_matrix = " << endl;
    cout << rotation_matrix << endl;
    cout << "eulerAngles = " << endl;
    cout << eulerAngles << endl;
    
    Mat_<double> r_l = (Mat_<double>(3, 1) << 0.78520514,0.0233998,0.00969251);//左摄像机的旋转向量
    Mat R_L;
    Rodrigues(r_l, R_L);
    eulerAngles = rotationMatrixToEulerAngles(R_L);
    cout << "R_L = " << endl;
    cout << R_L << endl;
    cout << "eulerAngles = " << endl;
    cout << eulerAngles << endl;
    eulerAngles(0) = 90.0;
    eulerAngles(1) = 0.0;
    eulerAngles(2) = 0.0;
    //cvRodrigues2(&pr_vec,&pR_matrix,0);
    return 0;
}

以下makefile用来在linux平台下编译成可执行文件，将它放在makefile的文件中，在终端中输入make即可完成编译：

SOURCE  := $(wildcard ./test_euler.cpp)
#wildcard把指定目录 ./ 下的所有后缀是cpp的文件全部展开。
DIR     := $(notdir $(SOURCE))
#notdir把展开的文件去除掉路径信息
OBJS    := $(patsubst %.cpp,%.o,$(DIR))
#patsubst把$(DIR)中的变量符合后缀是.cpp的全部替换成.o，

# target you can change test to what you want
TARGET  := test
$(CC) = gcc
$(CXX) = g++

#ifeq ($(CC),aarch64-linux-gcc)
#else ifeq ($(findstring arm-poky-linux-gnueabi-gcc,$(CC)), arm-poky-linux-gnueabi-gcc)

LDFLAGS := -L. -L$(SDKTARGETSYSROOT)/usr/lib 
INCLUDE := -I$(SDKTARGETSYSROOT)/usr/include/ -L./lib
LIBS    :=   -lm -lopencv_core -lopencv_highgui \
            -lopencv_imgproc -lopencv_calib3d -lopencv_imgcodecs \
            -lopencv_flann -lopencv_videoio -ljpeg


DEFINES := -Wl,-rpath=./lib

CFLAGS  := -g -Wall $(DEFINES) $(INCLUDE)

CXXFLAGS:= $(CFLAGS)

all : $(TARGET)

$(TARGET) : $(OBJS)
    $(CXX) $(CXXFLAGS) -o $@ $(OBJS) $(LDFLAGS) $(LIBS)
    echo $(CC) $(TARGET) ok

objs : $(OBJS)

rebuild: clean everything

clean :
    rm -rf *.o
    rm -rf $(TARGET)

完整的测试Python程序如下：

#!/usr/bin/env python3
# coding:utf-8

import cv2
import numpy as np
import time
import threading
import os
import re    
import subprocess
import random
import math

# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(angles1) :
    theta = np.zeros((3, 1), dtype=np.float64)
    theta[0] = angles1[0]*3.141592653589793/180.0
    theta[1] = angles1[1]*3.141592653589793/180.0
    theta[2] = angles1[2]*3.141592653589793/180.0
    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 ))
    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
    #print('dst:', R)
    x = x*180.0/3.141592653589793
    y = y*180.0/3.141592653589793
    z = z*180.0/3.141592653589793
    rvecstmp = np.zeros((1, 1, 3), dtype=np.float64)
    rvecs,_ = cv2.Rodrigues(R, rvecstmp)
    #print()
    return R,rvecs,x,y,z

def rotationMatrixToEulerAngles(rvecs):
    R = np.zeros((3, 3), dtype=np.float64)
    cv2.Rodrigues(rvecs, R)
    sy = math.sqrt(R[2,1] * R[2,1] +  R[2,2] * R[2,2])
    sz = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    print('sy=',sy, 'sz=', sz)
    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
    #print('dst:', R)
    x = x*180.0/3.141592653589793
    y = y*180.0/3.141592653589793
    z = z*180.0/3.141592653589793
    return x,y,z

if __name__=='__main__':
    eulerAngles = np.zeros((3, 1), dtype=np.float64)
    eulerAngles[0] = 15.0
    eulerAngles[1] = 25.0
    eulerAngles[2] = 35.0
    R,rvecstmp,x,y,z = eulerAnglesToRotationMatrix(eulerAngles)
    print('输入欧拉角：\n', eulerAngles)
    print('旋转转矩：\n', R)
    #rvecstmp[0] = -2.100418
    #rvecstmp[1] = -2.167796
    #rvecstmp[2] = 0.273330 0.75467396][-0.00747155][ 0.00896453
    #rvecstmp[0] = 0.75467396
    #rvecstmp[1] = -0.00747155
    #rvecstmp[2] = 0.00896453
    
    print('旋转向量：\n', rvecstmp)
    print('计算后的欧拉角：\n', rotationMatrixToEulerAngles(rvecstmp))