Rotation Matrix To Euler Angles
来源:互联网 发布:电脑截动图用什么软件 编辑:程序博客网 时间:2024/06/05 20:20
Learn OpenCV
OpenCV examples and tutorials ( C++ / Python )
- Home
- About
- Resources
- AI Consulting
- Courses
Rotation Matrix To Euler Angles
June 4, 2016 By Satya Mallick
In this post I will share code for converting a 3×3 rotation matrix to Euler angles and vice-versa.
3D rotations matrices can make your head spin. I know it is a bad pun but truth can sometimes be very punny!
A rotation matrix has three degrees of freedom, and mathematicians have exercised their creative freedom to represent a 3D rotation in every imaginable way — using three numbers, using four numbers, using a 3×3 matrix. And there are a ton of different ways of representing a rotation as three numbers and a few ways to represent it as 4 numbers.
For example, rotation in 3D can be represented as three angles that specify three rotations applied successively to the X, Y and Z axes. But you could also represent the same rotation as three angles applied successively to Z, Y and X axes. These angles are called Euler angles or Tait–Bryan angles. In the original Euler angle formulation, a rotation is described by successive rotations about the Z, X and again Z axes ( or for that matter Y-X-Y, or Z-Y-Z ). When the rotation is specified as rotations about three distinct axes ( e.g. X-Y-Z ) they should be called Tait–Bryan angles, but the popular term is still Euler angles and so we are going to call them Euler angles as well.
There are six possible ways you can describe rotation using Tait–Bryan angles — X-Y-Z, X-Z-Y, Y-Z-X, Y-X-Z, Z-X-Y, Z-Y-X. Now you are thinking, the choice is easy. Let’s just choose X-Y-Z. Right ? Wrong. The industry standard is Z-Y-X because that corresponds to yaw, pitch and roll.
There are additional ambiguities while defining rotation matrices. E.g. Given a point, you can think of this point as a row vector or a column vector . If you use a row vector, you have to post-multiply the 3×3 rotation matrix and if you use the column vector representation you have to pre-multiply the rotation matrix to rotate the point. These two rotation matrices are not the same ( they are the transpose of each other ).
My point is that there is no standard way to convert a rotation matrix to Euler angles. So, I decided to be (almost) consistent with the MATLAB implementation ofrotm2euler.m. The only difference is that they return the Euler angles with the rotation about z first and x last. My code returns x first.
Euler Angles to Rotation Matrices
The easiest way to think about 3D rotation is the axis-angle form. Any arbitrary rotation can be defined by an axis of rotation and an angle the describes the amount of rotation. Let’s say you want to rotate a point or a reference frame about the x axis by angle . The rotation matrix corresponding to this rotation is given by
Rotations by and about the y and z axes can be written as
A rotation about any arbitrary axis can be written in terms of successive rotations about the Z, Y, and finally X axes using the matrix multiplication shown below.
In this formulation , and are the Euler angles. Given these three angles you can easily find the rotation matrix by first finding, and and then multiply them to obtain . The code below shows an example
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;}
Python
# Calculates Rotation Matrix given euler angles.def eulerAnglesToRotationMatrix(theta) : R_x = np.array([[1, 0, 0 ], [0, math.cos(theta[0]), -math.sin(theta[0]) ], [0, math.sin(theta[0]), math.cos(theta[0]) ] ]) R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ], [0, 1, 0 ], [-math.sin(theta[1]), 0, math.cos(theta[1]) ] ]) R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], [math.sin(theta[2]), math.cos(theta[2]), 0], [0, 0, 1] ]) R = np.dot(R_z, np.dot( R_y, R_x )) return R
Convert a Rotation Matrix to Euler Angles in OpenCV
Converting a rotation matrix to Euler angles is a bit tricky. The solution is not unique in most cases. Using the code in the previous section you can verify that rotation matrices corresponding to Euler angles ( or in degrees) and ( or in degrees) are actually the same even though the Euler angles look very different. The code below shows a method to find the Euler angles given the rotation matrix. The output of the following code should exactly match the output of MATLAB’srotm2euler but the order of x and z are swapped.
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); }
Python
# Checks if a matrix is a valid rotation matrix.def isRotationMatrix(R) : Rt = np.transpose(R) shouldBeIdentity = np.dot(Rt, R) I = np.identity(3, dtype = R.dtype) n = np.linalg.norm(I - shouldBeIdentity) return n < 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 ).def rotationMatrixToEulerAngles(R) : assert(isRotationMatrix(R)) sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) singular = sy < 1e-6 if not singular : x = math.atan2(R[2,1] , R[2,2]) y = math.atan2(-R[2,0], sy) z = math.atan2(R[1,0], R[0,0]) else : x = math.atan2(-R[1,2], R[1,1]) y = math.atan2(-R[2,0], sy) z = 0 return np.array([x, y, z])
Subscribe & Download Code
If you liked this article and would like to download code (C++ and Python) and example images used in this blog, pleasesubscribe to our newsletter. You will also receive a free Computer Vision Resource guide. In our newsletter we share OpenCV tutorials and examples written in C++/Python, and Computer Vision and Machine Learning algorithms and news.
Subscribe Now
Hi. Thanks for your code! I would like to use it with Unity3d, but Unity rotates in order ZXY order. How can I adapt the calculations to output that order? It would be great if you could help me out!
I’m no expert. But it seems odd to test for a singularity near zero, then continue to use the near zero value (sy). Is this correct?
- Андрей Серебро
the trick is that atan2 gracefully deals with tangents of angles near PI/2, so it’s perfectly fine.
Hi. I need your advise please. I’m doing a Project, the Project is about inclinometer on ships, which roll and pitch angles must be measured using 3-axis accelerometer and a 3-axis gyroscope. can you advise me how can i begin with that. I’m a beginner in this field, so any advise can be useful.
Hi, Is it any different from opencv implementation that can be found here:
https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp
https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.h
functions:
// Converts a given Euler angles to Rotation Matrix
cv::Mat euler2rot(const cv::Mat & euler
and
// Converts a given Rotation Matrix to Euler angles
cv::Mat rot2euler(const cv::Mat & rotationMatrix)
- Siddhant Mehta
Thanks phryniszak for mentioning the references…It was useful for me..only the conversion given there are working for me…
Search
Computer Vision Resources
To learn more about OpenCV books, libraries, and web APIs download our free resource guide.
Download
Follow
Satya Mallick
I am an entrepreneur who loves Computer Vision and Machine Learning. I have a dozen years of experience (and a Ph.D.) in the field.
I am a co-founder of TAAZ Inc where the scalability, and robustness of our computer vision and machine learning algorithms have been put to rigorous test by more than 100M users who have tried our products.Read More…
DISCLAIMER
RECENT POSTS
- Install Dlib on Windows
- Install Dlib on Ubuntu
- Install OpenCV3 on Ubuntu
- Read, Write and Display a video using OpenCV ( C++/ Python )
- Install Dlib on MacOS
Copyright © 2017 · Big Vision LLC
- Rotation Matrix To Euler Angles
- Computing Euler angles from a rotation matrix (翻译)
- 欧拉角(Euler angles)
- 欧拉角(Euler Angles)
- 欧拉角(Euler angles)
- Rotation Matrix
- OpenGL Angles to Axes
- 关于Euler angles(欧拉角)的问题
- To get the View matrix from the rotation,translation,scale
- Matrix转rotation
- HackerRank [Algo] Matrix Rotation
- 根据Matrix获取Rotation
- Gimbal Lock(万向节锁)在 Euler Angles(欧拉角)中的体现
- Rodrigues' Rotation Matrix(罗德里格旋转矩阵)
- The Mathematics of the 3D Rotation Matrix
- Get translation and rotation matrix of an object
- My Solution to Project Euler in Ruby
- My Solution to Project Euler in Perl
- PLSQL developer 连接不上64位Oracle 的解决方法
- 2017年第0届浙江工业大学之江学院程序设计竞赛决赛—G(数论)
- Android Studio 出现 Gradle's dependency cache may be corrupt 错误解决办法
- android monkey测试
- URAL
- Rotation Matrix To Euler Angles
- poj 2386(深搜或广搜)
- 四元数(Quaternion)和旋转
- 面试题-2017/6/19
- CG在vs中配置方法
- 实现一个简单计算器,表达式为字符串表示
- 点击li弹出对应的index
- Spring MVC工作流程详解
- mybatis学习之添加新用户例子