美文网首页一起折腾传感器
欧拉角、旋转向量和旋转矩阵的相互转换

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

作者: shaniadolphin | 来源:发表于2019-06-08 19:27 被阅读0次

欧拉角转旋转矩阵

对于两个三维点 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))

相关文章

  • SLAM学习笔记3

    三维空间刚体运动，笔记内容有向量内积，向量外积，欧氏变换，旋转向量，欧拉角，旋转矩阵，四元数以及它们的转换关系，代...

  • 三维空间的刚体运动

    旋转矩阵 对于同一个向量a(向量并没有随着坐标系的旋转而发生运动) 旋转向量与欧拉角 旋转矩阵R有九个元素，但仅有...

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

    欧拉角转旋转矩阵 对于两个三维点 ，，由点经过旋转矩阵旋转到，则有： 任何一个旋转可以表示为依次绕着三个旋转轴旋三...

  • 机器臂matlab篇

    1.Matlab ——旋转矩阵，四元数，欧拉角之间的转换 matlab练习程序（旋转矩阵、欧拉角、四元数互转） 2...

  • 【译】Learn OpenCV之Rotation Matrix

    本文要介绍的是的旋转矩阵与欧拉角(Euler Angles)之间的相互转换方法。 本文其实和OpenCV关系不大，...

  • 欧拉角

    个人理解思路 欧拉角===》旋转矩阵（一般直接得到单位正交阵）==》旋转向量（一个轴，一个角）==》旋转矢量（一般...

  • Eigen库旋转矩阵旋转向量欧拉角四元素

    旋转矩阵直接使用 Matrix3d 或 Matrix3f 旋转向量使用 AngleAxis 旋转矩阵和旋转向量的转...

  • 姿态、角度、位置

    欧拉角pitch、yaw，roll的理解 旋转变换（一）旋转矩阵

  • UE4 三种旋转（二）

    UE4 三种旋转（二） UE4 中旋转操作常用的方式：欧拉角、四元数和旋转矩阵。 欧拉角 四元数 利用四元数进行旋...

  • vlsam旋转向量与旋转矩阵—Apple的学习笔记

    旋转可以用旋转矩阵（9个量）或者旋转向量(3个量)，或者四元素（4个量）表示。旋转矩阵中的元素不是相互独立的，这在...

网友评论

    本文标题：欧拉角、旋转向量和旋转矩阵的相互转换

    本文链接：https://www.haomeiwen.com/subject/ncosxctx.html

    延伸阅读

    深度阅读

    • 您也可以注册成为美文阅读网的作者，发表您的原创作品、分享您的心情！

    栏目导航

    热点阅读

    一起折腾传感器

    关于我们|服务条款|联系我们|欧拉角、旋转向量和旋转矩阵的相互转换|投稿指南|网站地图|RSS订阅|排版工具|手机版

    提供经典美文摘抄,优美散文欣赏,现代诗歌精选,短篇小说,心情随笔,表白情书范文,故事会在线阅读欣赏

    Copyright © 2014-2023 Haomeiwen.com All Rights Reserved. 好美文阅读网 版权所有

    备案信息：桂公网安备 45052102000051号 · 桂ICP备13007215号-3

    本站所收录作品、热点评论等信息部分来源互联网，目的只是为了系统归纳学习和传递资讯

    所有作品版权归原创作者所有，与本站立场无关，如不慎侵犯了你的权益，请联系我们告知，我们将做删除处理！