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

最新推荐文章于 2024-05-16 14:57:12 发布

YOULANSHENGMENG

最新推荐文章于 2024-05-16 14:57:12 发布

阅读量1.9k

点赞数 3

分类专栏： OpenCV 文章标签：矩阵 opencv

本文链接：https://blog.csdn.net/YOULANSHENGMENG/article/details/129547954

版权

OpenCV 专栏收录该内容

5 篇文章 1 订阅

订阅专栏

一、什么是欧拉角

在3D 空间中，表示物体的旋转可以由三个欧拉角来表示：
pitch围绕X轴旋转，叫俯仰角。
yaw围绕Y轴旋转，叫偏航角。
roll围绕Z轴旋转，叫翻滚角。
这三个角的顺序对旋转结果有影响。

欧拉角

此处得到结论：自旋转的“先转的放前面”

二、旋转矩阵转欧拉角

将旋转矩阵表示如下：
这里写图片描述
则可以如下表示欧拉角：

#include <opencv2/opencv.hpp>

#include <iostream>
#include <chrono>
#include <string.h>
using namespace std;
using namespace cv;


/**
 * 功能： 1. 检查是否是旋转矩阵
**/
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;
}

/**
 * 功能： 1. 通过给定的旋转矩阵计算对应的欧拉角
**/
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);
}

int main()
{
	double k[9] = { 0.99979294,0.01240282,0.01613191, -0.0123678,0.99992094,-0.00226879,-0.01615878,0.0020688,0.9998673};
	Mat km = Mat(3, 3, CV_64FC1, k);
	cout << km<<endl;
	bool kk = isRotationMatrix(km);
	cout << kk << endl;
	//cout << km.at<double>(0, 0) << endl;
	Vec3f point=rotationMatrixToEulerAngles(km);
	cout << point << endl;
	
	return 0;
}

三、欧拉角转旋转矩阵

如果将每一个角度用旋转矩阵表示如下：
这里写图片描述
所以，容易得到，欧拉角转旋转矩阵如下：

/**
欧拉角计算对应的旋转矩阵
**/
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // 计算旋转矩阵的X分量
    Mat R_x = (Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );
 
 
    // 计算旋转矩阵的Y分量
    Mat R_y = (Mat_<double>(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );
 
 
    // 计算旋转矩阵的Z分量
    Mat R_z = (Mat_<double>(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);
 
 
    // 合并 
    Mat R = R_z * R_y * R_x;
 
 
    return R;
}