【計算機視覺】旋轉矩陣與歐拉角相互轉換

 

問題

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 <[e-mail]>
 * 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;
}

 

 

參考

1. https://blog.csdn.net/lircsszz/article/details/80118051;

2. learnopencv_Rotation Matrix To Euler Angles；

3. https://blog.csdn.net/u012525096/article/details/78890463;

4. https://blog.csdn.net/qq_31806429/article/details/78844305;

5. http://answers.opencv.org/question/16796/computing-attituderoll-pitch-yaw-from-solvepnp/?answer=52913#post-id-52913;

6. https://github.com/lincolnhard/head-pose-estimation/issues/17;

完