這部分代碼我摘自於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<double>(0,0) + R.at<double>(1,1) + R.at<double>(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_<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;
}
return Vec3f(x, y, z);
}
#define _USE_MATH_DEFINES #include <cmath> 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 <cstdlib> #include <cstdio> #include <cmath> #include <iostream> #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<float>(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:"<<cv::Mat(4,1,CV_32FC1,q).t()<<endl; return cv::Mat(4,1,CV_32FC1,q).clone(); } //四元數得到歐拉角 cv::Mat Quaternion2Euler(float *q) { float w = q[0]; float x = q[1]; float y = q[2]; float z = q[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[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<float>(0); float x = q.at<float>(1); float y = q.at<float>(2); float z = q.at<float>(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<float>(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<float>(0); angle[1] = Angle.at<float>(1); angle[2] = Angle.at<float>(2); return Euler2Quaternion(angle).clone(); } // 由旋轉四元數推導出矩陣 cv::Mat Quaternion2Matrix (cv::Mat q) { float w = q.at<float>(0); float x = q.at<float>(1); float y = q.at<float>(2); float z = q.at<float>(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"<<R<<endl; cv::Mat Rvec(3,1,CV_32FC1); Rodrigues(R.rowRange(0,3).colRange(0,3),Rvec); cout<< "\n Rodrigues:"<<Rvec.t()*57.3f<<endl; cv::Mat a1 = Matrix2Euler(R); cout<< "\n Matrix 2 Eular:"<<a1.t()*57.3f<<endl; //矩陣推四元數,四元數推矩陣,OK cout<<"\n----------矩陣--四元數-------------"<<endl; Mat quaternion = Matrix2Quaternion(R); Mat matrix = Quaternion2Matrix(quaternion); Mat quaternion2 = Matrix2Quaternion(matrix); cout<<" quaternion(qw qx qy qz):"<<quaternion.t()<<endl; cout<<" matrix:\n"<<matrix<<endl; cout<<" quaternion--:"<<quaternion2.t()<<endl; cout<<"\n---------四元數--歐拉角------------"<<endl; //四元數推歐拉角，歐拉角推四元數 Mat euler = Quaternion2Euler(quaternion); Mat qq = Euler2Quaternion(euler); Mat euler2 = Quaternion2Euler(qq); cout<<" euler:"<<euler.t()*57.3f<<endl; cout<<" quaternion 2:"<<qq.t()<<endl; cout<<" euler2:"<<euler2.t()*57.3f<<endl; cout<<"\n---------歐拉角--矩陣----------"<<endl; cv::Mat matrix2 = Euler2Matrix(euler2); cv::Mat euler3 = Matrix2Euler(matrix2); cout<< " euler:"<<euler2.t()*57.3f<<endl; cout<< " matirx:\n"<<matrix2<<endl; cout<< " euler--:"<<euler3.t()*57.3f<<endl; cv::Mat rod; Rodrigues(matrix2.rowRange(0,3).colRange(0,3),rod); cout<< " rod angle1:"<<rod.t()*57.3f<<endl; Rodrigues(matrix.rowRange(0,3).colRange(0,3),rod); cout<< " rod angle2:"<<rod.t()*57.3f<<endl; cv::Mat T = R.inv()*matrix; cout << " T1:"<<Matrix2Euler(T).t()*57.3f<<endl; T = R.inv()*matrix2; cout << " T2:"<<Matrix2Euler(T).t()*57.3f<<endl; }