[学习opencv]基于OpenCV的四元数、旋转矩阵和欧拉角互相转换

这部分代码我摘自于google中搜索出来的博客，当然，在格式或者风格上不太统一。如果你想看到更纯粹的OpenCV版本，请戳基于OpenCV的四元数、旋转矩阵和欧拉角互相转换(二)。

四元数转旋转矩阵

void getRotation(double *Quaternion, double *rt_mat)
{
  rt_mat[0] = 1 - 2 * (Quaternion[2] * Quaternion[2]) - 2 * (Quaternion[3] * Quaternion[3]);
  rt_mat[1] = 2 * Quaternion[1] * Quaternion[2] - 2 * Quaternion[0] * Quaternion[3];
  rt_mat[2] = 2 * Quaternion[1] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[2];
  rt_mat[3] = 2 * Quaternion[1] * Quaternion[2] + 2 * Quaternion[0] * Quaternion[3];
  rt_mat[4] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[3] * Quaternion[3]);
  rt_mat[5] = 2 * Quaternion[2] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[1];
  rt_mat[6] = 2 * Quaternion[1] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[2];
  rt_mat[7] = 2 * Quaternion[2] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[1];
  rt_mat[4] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[2] * Quaternion[2]);
}

旋转矩阵转四元数

void getQuaternion(Mat R, double Q[])
{
    double trace = R.at(0,0) + R.at(1,1) + R.at(2,2);
if (trace > 0.0) 
{
    double s = sqrt(trace + 1.0);
    Q[3] = (s * 0.5);
    s = 0.5 / s;
    Q[0] = ((R.at<double>(2,1) - R.at<double>(1,2)) * s);
    Q[1] = ((R.at<double>(0,2) - R.at<double>(2,0)) * s);
    Q[2] = ((R.at<double>(1,0) - R.at<double>(0,1)) * s);
} 

else 
{
    int i = R.at<double>(0,0) < R.at<double>(1,1) ? (R.at<double>(1,1) < R.at<double>(2,2) ? 2 : 1) : (R.at<double>(0,0) < R.at<double>(2,2) ? 2 : 0); 
    int j = (i + 1) % 3;  
    int k = (i + 2) % 3;

    double s = sqrt(R.at<double>(i, i) - R.at<double>(j,j) - R.at<double>(k,k) + 1.0);
    Q[i] = s * 0.5;
    s = 0.5 / s;

    Q[3] = (R.at<double>(k,j) - R.at<double>(j,k)) * s;
    Q[j] = (R.at<double>(j,i) + R.at<double>(i,j)) * s;
    Q[k] = (R.at<double>(k,i) + R.at<double>(i,k)) * s;
}

}

欧拉角转旋转矩阵

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_(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;
}
return Vec3f(x, y, z);

}

#define _USE_MATH_DEFINES
#include 

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

   
   
   
   

            
                                        
    
    
    
    

                    
     
                    
     
                        
      
                    
     
                                            
     
     
     
     
此处的代码都会有双版本：第一个版本假设输入是float数组，第二个版本假设输入是cv::Mat矩阵。

     
     
     
     
#include 
#include 
#include 
#include 
#include "opencv2/opencv.hpp"

using namespace std;
using namespace cv;

#define PI 3.141592653

//旋转矩阵得到四元数
cv::Mat Matrix2Quaternion(cv::Mat matrix)
{
  float tr, qx, qy, qz, qw;

  // 计算矩阵轨迹
  float a[4][4] = {0};
  for(int i=0;i<4;i++)
    for(int j=0;j<4;j++)
      a[i][j]=matrix.at(i,j);
  
  // I removed + 1.0f; see discussion with Ethan
  float trace = a[0][0] + a[1][1] + a[2][2]; 
  if( trace > 0 ) {
    // I changed M_EPSILON to 0
    float s = 0.5f / sqrtf(trace+ 1.0f);
    qw = 0.25f / s;
    qx = ( a[2][1] - a[1][2] ) * s;
    qy = ( a[0][2] - a[2][0] ) * s;
    qz = ( a[1][0] - a[0][1] ) * s;
  } else {
    if ( a[0][0] > a[1][1] && a[0][0] > a[2][2] ) {
      float s = 2.0f * sqrtf( 1.0f + a[0][0] - a[1][1] - a[2][2]);
      qw = (a[2][1] - a[1][2] ) / s;
      qx = 0.25f * s;
      qy = (a[0][1] + a[1][0] ) / s;
      qz = (a[0][2] + a[2][0] ) / s;
    } else if (a[1][1] > a[2][2]) {
      float s = 2.0f * sqrtf( 1.0f + a[1][1] - a[0][0] - a[2][2]);
      qw = (a[0][2] - a[2][0] ) / s;
      qx = (a[0][1] + a[1][0] ) / s;
      qy = 0.25f * s;
      qz = (a[1][2] + a[2][1] ) / s;
    } else {
      float s = 2.0f * sqrtf( 1.0f + a[2][2] - a[0][0] - a[1][1] );
      qw = (a[1][0] - a[0][1] ) / s;
      qx = (a[0][2] + a[2][0] ) / s;
      qy = (a[1][2] + a[2][1] ) / s;
      qz = 0.25f * s;
    }    
  }

  float q[] = {qw,qx,qy,qz};
  //cout<< "\n quaternion:"< 0.4999f) {
    ret[2] = 2.0f * atan2f(x, w);
    ret[1] = PI/2;
    ret[0] = 0.0f;
    return cv::Mat(3,1,CV_32FC1,ret).clone();
  }
  if (test < -0.4999f) {
    ret[2] = 2.0f * atan2f(x, w);
    ret[1] = -PI/2;
    ret[0] = 0.0f;
    return cv::Mat(3,1,CV_32FC1,ret).clone();
  }
  float sqx = x * x;
  float sqy = y * y;
  float sqz = z * z;
  ret[2] = atan2f(2.0f * y * w - 2.0f * x * z, 1.0f - 2.0f * sqy - 2.0f * sqz);
  ret[1] = asin(2.0f * test);
  ret[0] = atan2f(2.0f * x * w - 2.0f * z * y, 1.0f - 2.0f * sqx - 2.0f * sqz);
      
  ret[0] *= 180/PI;
  ret[1] *= 180/PI;
  ret[2] *= 180/PI;

  return cv::Mat(3,1,CV_32FC1,ret).clone();
}

//四元数得到欧拉角
cv::Mat Quaternion2Euler(cv::Mat q)
{
  float w = q.at(0);
  float x = q.at(1);
  float y = q.at(2);
  float z = q.at(3);

  float ret[3] = {0};
  //cv::Mat ret(3,1,CV_32FC1);
 
  float test = x*y + z*w;
  if (test > 0.4999f) {
    ret[2] = 2.0f * atan2f(x, w);
    ret[1] = PI/2;
    ret[0] = 0.0f;
    return cv::Mat(3,1,CV_32FC1,ret).clone();
  }
  if (test < -0.4999f) {
    ret[2] = 2.0f * atan2f(x, w);
    ret[1] = -PI/2;
    ret[0] = 0.0f;
    return cv::Mat(3,1,CV_32FC1,ret).clone();
  }
  float sqx = x * x;
  float sqy = y * y;
  float sqz = z * z;

  ret[0] = atan2f(2.0f*(y*w-x*z), 1.0f-2.0f*(sqy+sqz));
  ret[1] = asin(2.0f * test);
  ret[2] = atan2f(2.0f*(x*w - y*z), 1.0f-2.0f*(sqx+sqz));

  return cv::Mat(3,1,CV_32FC1,ret).clone();
}

cv::Mat Matrix2Euler(cv::Mat matrix)
{
  cv::Mat q = Matrix2Quaternion(matrix);
  cv::Mat angle = Quaternion2Euler(q);
  return angle.clone();

  float m[4][4] = {0};
  for(int a=0;a<4;a++)
    for(int b=0;b<4;b++)
      m[a][b]=matrix.at(a,b);
  
  float a[3];
  a[0] = atan2f(m[2][1],m[2][2]) *180/PI;
  a[1] = atan2f(-m[2][0], sqrtf(m[2][1]*m[2][1] +  m[2][2]*m[2][2])) *180/PI;
  a[2] = atan2f(m[1][0], m[0][0]) *180/PI;
  return cv::Mat(3,1,CV_32FC1,a).clone();
}

// 由欧拉角创建四元数
cv::Mat Euler2Quaternion(float *angle)
{
  float heading = angle[0];
  float attitude = angle[1];
  float bank = angle[2];

  float c1 = cos(heading/2);
  float s1 = sin(heading/2);
  float c2 = cos(attitude/2);
  float s2 = sin(attitude/2);
  float c3 = cos(bank/2);
  float s3 = sin(bank/2);
  float c1c2 = c1*c2;
  float s1s2 = s1*s2;
  float w =c1c2*c3 - s1s2*s3;
  float x =c1c2*s3 + s1s2*c3;
  float y =s1*c2*c3 + c1*s2*s3;
  float z =c1*s2*c3 - s1*c2*s3;
  float q[4] = {w,x,y,z};
  cv::Mat ret(4,1,CV_32FC1,q);
  return ret.clone();

}

cv::Mat Euler2Quaternion(cv::Mat Angle)
{
  //angle:roll pitch yaw
  //    q:w x y z
  float angle[3];
  angle[0] = Angle.at(0);
  angle[1] = Angle.at(1);
  angle[2] = Angle.at(2);

  return Euler2Quaternion(angle).clone();
}

// 由旋转四元数推导出矩阵
cv::Mat Quaternion2Matrix (cv::Mat q)
{
  float w = q.at(0);
  float x = q.at(1);
  float y = q.at(2);
  float z = q.at(3);

  float xx = x*x;
  float yy = y*y;
  float zz = z*z;
  float xy = x*y;
  float wz = w*z;
  float wy = w*y;
  float xz = x*z;
  float yz = y*z;
  float wx = w*x;

  float ret[4][4];
  ret[0][0] = 1.0f-2*(yy+zz);
  ret[0][1] = 2*(xy-wz);
  ret[0][2] = 2*(wy+xz);
  ret[0][3] = 0.0f;
 
  ret[1][0] = 2*(xy+wz);
  ret[1][1] = 1.0f-2*(xx+zz);
  ret[1][2] = 2*(yz-wx);
  ret[1][3] = 0.0f;
 
  ret[2][0] = 2*(xz-wy);
  ret[2][1] = 2*(yz+wx);
  ret[2][2] = 1.0f-2*(xx+yy);
  ret[2][3] = 0.0f;
 
  ret[3][0] = 0.0f;
  ret[3][1] = 0.0f;
  ret[3][2] = 0.0f;
  ret[3][3] = 1.0f;
 
  return cv::Mat(4,4,CV_32FC1,ret).clone();
}

//欧拉角转旋转矩阵,Euler:弧度表示
cv::Mat Euler2Matrix(float *angle)
{
  cv::Mat q = Euler2Quaternion(angle);
  return Quaternion2Matrix(q).clone();
}

//欧拉角转旋转矩阵
cv::Mat Euler2Matrix(cv::Mat angle)
{
  cv::Mat q = Euler2Quaternion(angle);
  return Quaternion2Matrix(q).clone();
}


int main()
{
  float r[] = {
    0.00316709,  0.999958,    0.00860068,  0.0146869 ,
    -0.05037308,  0.00874934, -0.99869215, -0.01471531,
    -0.99872545,  0.0027297,   0.05039868, -0.01815286,
    0, 0, 0, 1
  };

  cv::Mat R(4,4,CV_32FC1,r);
  cout<< "\n Matrix:\n"<