欧拉角、旋转向量和旋转矩阵的相互转换

提示：若只有一个方向的旋转则可以用下面公司计算旋转角度（幅值）：

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

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

    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程序如下：

import tkinter as tk
import tkinter.font as tkFont
import tkinter.messagebox
import tkinter.ttk as ttk
from tkinter import filedialog
import cv2
import numpy as np
import time
from PIL import Image,ImageTk
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
    rvecs = 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[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

if(__name__=='__main__'):
    eulerAngles = np.zeros((3, 1), dtype=np.float64)
    eulerAngles[0] = 45.0
    eulerAngles[1] = 0.0
    eulerAngles[2] = 0.0
    R,rvecstmp,x,y,z = eulerAnglesToRotationMatrix(eulerAngles)
    #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(rvecs)
    print(rotationMatrixToEulerAngles(rvecs))

文章转载地址：https://www.jianshu.com/p/5e130c04a602