Dlib姿态估计——旋转矩阵与欧拉角互转
来源:互联网 发布:symbian3软件 编辑:程序博客网 时间:2024/06/18 05:08
原文地址:https://www.learnopencv.com/rotation-matrix-to-euler-angles/
简介
在这篇文章中,我将分享将一个3×3旋转矩阵转换成欧拉角的代码,反之亦然。3D旋转矩阵可以让你的头旋转。 我知道这是一个坏的双关语,但真相有时可能是非常小的!
一个旋转矩阵有三个自由度,数学家已经行使了他们的创造自由,以每个想象的方式来表示3D旋转——或使用三个数字、或使用四个数字、或使用一个3×3矩阵。 还有很多不同的方式用三个数字表示一个旋转或用四个数字的一些方法来表示旋转。
例如,3D中的旋转可以表示为三个角度,可以将其表示在X、Y、Z三个轴上。但是,这三个角度也可以表示到Z,Y和X轴上(表示方法不同)。这些角度被称为欧拉角或Tait–Bryan角。在原始欧拉角公式中,通过围绕Z,X轴和再对Z轴(或者对于Y-X-Y或Z-Y-Z)的连续旋转来描述旋转。当旋转被指定为围绕三个不同的轴(例如X-Y-Z)的旋转时,它们应该被称为泰特—布赖恩(Tait-Bryan)角,但是流行的术语仍然是欧拉角,所以我们也将它们称为欧拉角。有六种可以用Tait-Bryan角度描述旋转的方法——X-Y-Z,X-Z-Y,Y-Z-X,Y-X-Z,Z-X-Y,Z-Y-X。现在你在想,选择很简单。 让我们选择X-Y-Z。 对 么? 答案是错误的。 行业标准是Z-Y-X,因为它对应于yaw偏航,pitch俯仰和roll滚转。
yaw偏航角
pitch俯仰角
roll滚转角
定义旋转矩阵时还有其他的含糊之处。给定一个点
我的观点是没有标准的方法来将旋转矩阵转换成欧拉角。所以,我决定(几乎)与MATLAB实现的rotm2euler.m
一致。唯一的区别是他们返回的欧拉角z轴是第一个和x轴是最后一个(Z-Y-X)。我的代码先返回x(X-Y-Z)。
欧拉角->旋转矩阵
对于3D旋转的最简单的方法是以轴角形式思考。任何旋转都可以由一个旋转轴来定义,一个角度可以描述旋转的量。比方说,你想旋转一个点或一个参考框架绕x轴旋转
关于任意轴的旋转
在这个公式中,
C++代码
// 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;}
在OpenCV中将旋转矩阵转换为欧拉角
将旋转矩阵转换成欧拉角是有点棘手的。该解决方案在大多数情况下不是唯一的。使用上一节中的代码,可以验证与欧拉角
C++
// 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);}