【计算机视觉】旋转矩阵与欧拉角相互转换-CFANZ编程社区

问题

1. 旋转矩阵的定义；

2. 欧拉角的定义;

3. 旋转矩阵和欧拉角之间的关系；

4. 旋转矩阵和欧拉角之间的转换及其代码；

5. decomposeProjectionMatrix函数中的欧拉角的单位是弧度还是角度，pitch/yaw/roll三者的顺序以及方向性又是如何对应的？

6. 已知欧拉角，如何计算得到变换后的3D坐标。

pitch = eulerAngles[0];
yaw   = eulerAngles[1]; 
roll  = eulerAngles[2];

不知道上边的赋值是否正确；

欧拉角和旋转矩阵之间的转换：

/*
 * Copyright (c) 2016 Satya Mallick <spmallick@learnopencv.com>
 * All rights reserved. No warranty, explicit or implicit, provided.
 */

#include "opencv2/opencv.hpp"
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */

using namespace cv;
using namespace std;

// 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;
    }
    return Vec3f(x, y, z);   
}

// 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;
}

int main(int argc, char** argv)
{
    // Initialize random number generator
    srand (time(NULL));    
    // Randomly generate Euler angles in Degrees.
    Vec3f eDegrees(rand() % 360 - 180.0, rand() % 360 - 180.0, rand() % 360 - 180.0);
    // Convert angles to radians
    Vec3f e = eDegrees * M_PI / 180.0;
    // Calculate rotation matrix
    Mat R = eulerAnglesToRotationMatrix(e);    
    // Calculate Euler angles from rotation matrix
    Vec3f e1 = rotationMatrixToEulerAngles(R);    
    // Calculate rotation matrix
    Mat R1 = eulerAnglesToRotationMatrix(e1);
    // Note e and e1 will be the same a lot of times
    // but not always. R and R1 should be the same always.    
    cout << endl << "Input Angles" << endl << e << endl;
    cout << endl << "R : " << endl << R << endl;
    cout << endl << "Output Angles" << endl << e1 << endl;
    cout << endl << "R1 : " << endl << R1 << endl;
}