Chinaunix首页 | 论坛 | 博客
  • 博客访问: 9413618
  • 博文数量: 1748
  • 博客积分: 12961
  • 博客等级: 上将
  • 技术积分: 20070
  • 用 户 组: 普通用户
  • 注册时间: 2009-01-09 11:25
个人简介

偷得浮生半桶水(半日闲), 好记性不如抄下来(烂笔头). 信息爆炸的时代, 学习是一项持续的工作.

文章分类

全部博文(1748)

文章存档

2024年(24)

2023年(26)

2022年(112)

2021年(217)

2020年(157)

2019年(192)

2018年(81)

2017年(78)

2016年(70)

2015年(52)

2014年(40)

2013年(51)

2012年(85)

2011年(45)

2010年(231)

2009年(287)

分类: Windows平台

2017-10-20 11:31:38

Intel Realsense SLAM module Output:

SLAM module建立地图的起始时刻(即调用Start的时刻),  SLAM module开始维护一个左手坐标系的世界坐标系.建立好的地图基于此坐标系.

重定位成功后, SLAM module通过 矩阵PoseMatrix4f pose;返回目前摄像头的姿态.

可以看出,返回结果包含两个部分.

1.位移向量T.  (X,Y,Z) --> pose.m_data[3], pose.m_data[7], pose.m_data[11]

2.旋转矩阵R.

此处理论上应该有 旋转矩阵 到Euler角的转换API,但暂时没有发现.

通过上述算法,得出欧拉角.(计算结果为弧度, 转化为度数 应该为 * 180/3.1415926)

如果确定了起始的世界坐标系与物理坐标系(地球东南西北)的旋转矩阵的话,还可以进行二次映射,把当前姿态换算成物理坐标系位置.


======================================================================================================================

Rotation Matrix To Euler Angles

JUNE 4, 2016 BY 

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.

This article contains code you can try out. If you want to download test scripts that use the functions included in this article, please subscribe to our newsletter by clicking .

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 .mathbf{(x, y, z)}, you can think of this point as a row vector [ x, y, z] or a column vector [ x, y, z]^T. 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 of rotm2euler.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 .theta_x. The rotation matrix corresponding to this rotation is given by

  .[ .mathbf{R_x} = .begin{bmatrix}     1       & 0 & 0 ..     0 & .cos(.theta_x) & -.sin(.theta_x)..     0 & .sin(.theta_x) & (.cos.theta_x) .end{bmatrix} .]

Rotations by .theta_y and .theta_z about the y and z axes can be written as

  .[ .mathbf{R_y} = .begin{bmatrix}     .cos(.theta_y)       & 0 & .sin(.theta_y) ..     0 & 1 & 0..     -.sin(.theta_y) & 0 & (.cos.theta_y) .end{bmatrix} .]

  .[ .mathbf{R_z} = .begin{bmatrix}     .cos(.theta_z) & -.sin(.theta_z) & 0 ..     .sin(.theta_z) & (.cos.theta_z) & 0 ..      0       & 0 & 1 .. .end{bmatrix} .]

A rotation .mathbf{R} 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.

  .[ .mathbf{R} = .mathbf{R_z R_y R_x} .]

In this formulation .theta_x.theta_y and .theta_z are the Euler angles. Given these three angles you can easily find the rotation matrix by first finding .mathbf{R_x}.mathbf{R_y} and .mathbf{R_z} and then multiply them to obtain .mathbf{R}. The code below shows an example

C++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
// 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_(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_(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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
# 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 [0.1920,  2.3736,   1.1170] ( or [[11, 136, 64] in degrees) and [-2.9496, 0.7679, -2.0246] ( or [-169, 44, -116] 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’s rotm2euler but the order of x and z are swapped.

C++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
// 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(0,0) * R.at(0,0) +  R.at(1,0) * R.at(1,0) );
 
    bool singular = sy < 1e-6; // If
 
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at(2,1) , R.at(2,2));
        y = atan2(-R.at(2,0), sy);
        z = atan2(R.at(1,0), R.at(0,0));
    }
    else
    {
        x = atan2(-R.at(1,2), R.at(1,1));
        y = atan2(-R.at(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);
     
     
     
}

Python

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
# 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, please  to our newsletter. You will also receive a free  guide. In our newsletter we share OpenCV tutorials and examples written in C++/Python, and Computer Vision and Machine Learning algorithms and news.


阅读(1871) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~