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

欧拉角转旋转矩阵

对于两个三维点 p_1(x_1,y_1,z_1)p_2(x_2,y_2,z_2),由点p_1经过旋转矩阵R旋转到p_2,则有:
R=\begin{bmatrix}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}

\begin{bmatrix}x_2 \\ y_2 \\ z_2 \end{bmatrix} = R\begin{bmatrix}x_1 \\ y_1 \\ z_1 \end{bmatrix}

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


R_x({\theta}) =\begin{bmatrix}1 & 0 & 0 \\ 0 & cos\theta & -sin\theta \\0 & sin\theta & cos\theta \end{bmatrix}
R_y({\theta}) =\begin{bmatrix}cos\theta & 0 & sin\theta \\ 0 & 1 & 0 \\ -sin\theta & 0 & cos\theta \end{bmatrix}
R_z({\theta}) =\begin{bmatrix}cos\theta & -sin\theta & 0 \\ sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}

因此欧拉角转旋转矩阵如下:
R =R_z(\phi)R_y(\theta)R_x(\psi)=\begin{bmatrix} cos\theta cos\phi & sin\psi sin\theta cos\phi - cos\psi sin\theta & cos \psi sin\theta cos\phi +sin\psi sin\phi \\cos\theta sin\phi & sin\psi sin\theta sin\phi+cos\psi cos\phi & cos\psi sin\theta sin\phi -sin\psi cos\theta \\ -sin\theta & sin\psi cos\theta & cos\psi cos\theta\end{bmatrix}
则可以如下表示欧拉角:
\theta_x=atan2(r_{32},r_{33})
\theta_y=atan2(-r_{31},\sqrt{r_{32}^2+r_{33}^2})
\theta_z=atan2(r_{21},r_{11})
以下代码用来实现旋转矩阵和欧拉角之间的相互变换。
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))
最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 204,293评论 6 478
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 85,604评论 2 381
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 150,958评论 0 337
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 54,729评论 1 277
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 63,719评论 5 366
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 48,630评论 1 281
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 38,000评论 3 397
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 36,665评论 0 258
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 40,909评论 1 299
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 35,646评论 2 321
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 37,726评论 1 330
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 33,400评论 4 321
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 38,986评论 3 307
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 29,959评论 0 19
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 31,197评论 1 260
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 44,996评论 2 349
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 42,481评论 2 342

推荐阅读更多精彩内容

  • 作为备用知识,memo 学过矩阵理论或者线性代数的肯定知道正交矩阵(orthogonal matrix)是一个非常...
    HappyPieBinLiu阅读 5,736评论 0 5
  • 概述 又研究了将近两个星期的3D图形到了我最想研究的地方了,因为欧拉角与四元数的原因导致OpenGL ES的研究进...
    神经骚栋阅读 7,740评论 12 40
  • 旋转矩阵 点,向量,坐标系 刚体不光有位置,还有自身的姿态.位置是指刚体在空间中的哪个地方,姿态是指刚体的朝向. ...
    南衍儿阅读 4,121评论 0 2
  • 欧拉角的定义 在写这篇博客之前,我搜索了网上很多关于欧拉角的定义,发现大部分引用自维基百科的定义,我这里也引述一下...
    AndrewFan阅读 2,853评论 3 12
  • 1、在研究 石墨文档 应用于微信小程序的过程中,本想直接同步到电脑上拷贝编辑,出现程序不响应; 2、出现轻微抱怨情...
    Jet李_e109阅读 145评论 0 0