slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用

一、有关坐标系变换的一些概述

坐标系的变换一般来说有四种方式表示变换：旋转向量、旋转矩阵、欧拉角和四元数，这里分别介绍下相应的原理及如何使用，最后附上相互转化的代码。

常用的一些如下：

旋转矩阵（3X3）:Eigen::Matrix3d

旋转向量（3X1）:Eigen::AngleAxisd

四元数（4X1）:Eigen::Quaterniond

平移向量（3X1）:Eigen::Vector3d

变换矩阵（4X4）:Eigen::Isometry3d

1、旋转向量

旋转向量其实想表示的就是绕着某个旋转轴转了某个角度。这里有旋转向量的两种表示方法：

1）简单的用一个3×1（3行1列，其中逗号表示一行）的向量来表示，该向量的方向就是旋转轴，它的模就是绕轴逆时针旋转的角度。

Eigen::Vector3d w(0.01,0.02,0.03);//w就是初始化的旋转向量。
1

2）使用Eigen库自带的AngleAxisd函数进行旋转向量的初始化，前面是旋转的矩阵，后面是旋转轴。

下面表示的是初始旋转矩阵，绕Z轴旋转45° （这个向量要求为单位向量）

Eigen::Matrix3d R=Eigen::AngleAxisd(M_PI/4,Eigen::Vector3d(0,0,1));
1

这两种旋转向量初始化的方法其实是一样的，只不过是表现形式不一样而已。

就比如绕Z轴旋转45°这个表示形式。使用第一种方法还可以表示为：

Eigen::Vector3d w(0,0,M_PI/4); //这个其实就和上面的那个绕Z轴旋转45°是一样的了。
1

2、旋转矩阵

旋转矩阵使用自身初始化自身的方式有以下两种：

1）使用旋转矩阵的函数来初始化为单位矩阵

//1.使用旋转矩阵的函数来初始化旋转矩阵
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
1
2
3

2）如果知道自己确切的各个参数，则可以利用下面的方法进行初始化

Eigen::Matrix3d R;
  R     <<   0,    -n_w(2),    n_w(1),
            n_w(2),     0,     -n_w(0),
            -n_w(1),  n_w(0),      0;
1
2
3
4

3、四元数

这里推荐两种初始化的方式：

1）方式一：实部为1，虚部为2,3,4.

Quaterniond q1(1, 2, 3, 4);                               // 第一种方式  实部为1 ，虚部234
1

2）方式二：第二种方式 实部为4 ，虚部1，2，3

Quaterniond q2(Vector4d(1, 2, 3, 4));      
1

4、欧拉角

（1）欧拉角的叫法：

欧拉角的叫法不固定，跟坐标轴的定义强相关。

在图1中，假设X是车头，Y是车左方，Z是车上方，那么绕X轴旋转得到的是roll，绕Y轴旋转得到的是pitch，绕Z轴得到的是yaw。

在图1中，假设Y是车头，X是车右方，Z是车上方，那么绕X轴旋转得到的是pitch，绕Y轴旋转得到的是roll，绕Z轴得到的是yaw。

（2）欧拉角正负：

如果是右手系，旋转轴正方向面对观察者时，逆时针方向的旋转是正、顺时针方向的旋转是负。

亦可这样描述：使用右手的大拇指指向旋转轴正方向，其他4个手指在握拳过程中的指向便是正方向。

如图1中的三次旋转都是正向旋转。

（3）欧拉角的范围：

这个要具体问题具体对待。

假如是车体坐标系（x-前，y-左，z-上），那么roll和pitch应该定义在（-90°，+90°），yaw应该定义在（-180°，+180°）。

假如是飞机坐标系，那么roll、pitch和yaw都应该定义在（-180°，+180°）。

Eigen中的默认范围roll、pitch和yaw都是（-180°，+180°）。

（4）明确旋转顺序和旋转轴：

对于x,y,z三个轴的不同旋转顺序一共有(x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z)六种组合，在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。

比如，先绕x轴旋转alpha，再绕y轴旋转beta；先绕y轴旋转beta，再绕x轴旋转alpha。这两种顺序得到的姿态是不一样的。

（5）内旋和外旋：

每次旋转是绕固定轴（一个固定参考系，比如世界坐标系）旋转，称为外旋。

每次旋转是绕自身旋转之后的轴旋转，称为内旋。

下图说明了内旋和外旋的区别。

二、各种表示方法之间的转换

1、旋转向量、旋转矩阵、四元素之间的转化

旋转向量和旋转矩阵之间的转化最基本的是原理是罗德里格斯公式，但是Eigen库和opencv库函数里面都有相

应的库函数使用罗德里格斯公式进行相互之间的转化。

1）、这里这三个之间的转化完全使用了Eigen库

#include <iostream>
#include <Eigen/Dense>


using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {



    //下面三个变量作为下面演示的中间变量

    AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1));
    Matrix3d t_R = t_V.matrix();
    Quaterniond t_Q(t_V);


    //对旋转向量（轴角）赋值的三大种方法

    //1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
    AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
    cout << "Rotation_vector1" << endl << V1.matrix() << endl;

    //2.使用旋转矩阵转旋转向量的方式

    //2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
    AngleAxisd V2;
    V2.fromRotationMatrix(t_R);
    cout << "Rotation_vector2" << endl << V2.matrix() << endl;

    //2.2 直接使用旋转矩阵来对旋转向量赋值
    AngleAxisd V3;
    V3 = t_R;
    cout << "Rotation_vector3" << endl << V3.matrix() << endl;

    //2.3 使用旋转矩阵来对旋转向量进行初始化
    AngleAxisd V4(t_R);
    cout << "Rotation_vector4" << endl << V4.matrix() << endl;

    //3. 使用四元数来对旋转向量进行赋值

    //3.1 直接使用四元数来对旋转向量赋值
    AngleAxisd V5;
    V5 = t_Q;
    cout << "Rotation_vector5" << endl << V5.matrix() << endl;

    //3.2 使用四元数来对旋转向量进行初始化
    AngleAxisd V6(t_Q);
    cout << "Rotation_vector6" << endl << V6.matrix() << endl;


//------------------------------------------------------

    //对四元数赋值的三大种方法（注意Eigen库中的四元数前三维是虚部,最后一维是实部）

    //1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
    Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以（0,0,1）为旋转轴，旋转45度
    //第一种输出四元数的方式
    cout << "Quaternion1" << endl << Q1.coeffs() << endl;

    //第二种输出四元数的方式
    cout << Q1.x() << endl << endl;
    cout << Q1.y() << endl << endl;
    cout << Q1.z() << endl << endl;
    cout << Q1.w() << endl << endl;

    //2. 使用旋转矩阵转四元數的方式

    //2.1 直接使用旋转矩阵来对旋转向量赋值
    Quaterniond Q2;
    Q2 = t_R;
    cout << "Quaternion2" << endl << Q2.coeffs() << endl;


    //2.2 使用旋转矩阵来对四元數进行初始化
    Quaterniond Q3(t_R);
    cout << "Quaternion3" << endl << Q3.coeffs() << endl;

    //3. 使用旋转向量对四元数来进行赋值

    //3.1 直接使用旋转向量对四元数来赋值
    Quaterniond Q4;
    Q4 = t_V;
    cout << "Quaternion4" << endl << Q4.coeffs() << endl;

    //3.2 使用旋转向量来对四元数进行初始化
    Quaterniond Q5(t_V);
    cout << "Quaternion5" << endl << Q5.coeffs() << endl;



//----------------------------------------------------

    //对旋转矩阵赋值的三大种方法

    //1.使用旋转矩阵的函数来初始化旋转矩阵
    Matrix3d R1=Matrix3d::Identity();
    cout << "Rotation_matrix1" << endl << R1 << endl;

    //2. 使用旋转向量转旋转矩阵来对旋转矩阵赋值

    //2.1 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
    Matrix3d R2;
    R2 = t_V.matrix();
    cout << "Rotation_matrix2" << endl << R2 << endl;

    //2.2 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
    Matrix3d R3;
    R3 = t_V.toRotationMatrix();
    cout << "Rotation_matrix3" << endl << R3 << endl;

    //3. 使用四元数转旋转矩阵来对旋转矩阵赋值

    //3.1 使用四元数的成员函数matrix()来对旋转矩阵赋值
    Matrix3d R4;
    R4 = t_Q.matrix();
    cout << "Rotation_matrix4" << endl << R4 << endl;

    //3.2 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
    Matrix3d R5;
    R5 = t_Q.toRotationMatrix();
    cout << "Rotation_matrix5" << endl << R5 << endl;

    return 0;


}

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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

2）这里opencv有罗德里格斯公式函数，完成旋转矩阵和旋转向量的互相转化的

#include <stdio.h>
#include <cv.h>
 
void main()
{
    int i;
    double r_vec[3]={-2.100418,-2.167796,0.273330};
    double R_matrix[9];
    CvMat pr_vec;
    CvMat pR_matrix;
 
    cvInitMatHeader(&pr_vec,1,3,CV_64FC1,r_vec,CV_AUTOSTEP);
    cvInitMatHeader(&pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);
    cvRodrigues2(&pr_vec, &pR_matrix,0);
 
    for(i=0; i<9; i++)
    {
        printf("%f\n",R_matrix[i]);
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

opencv的另外一种变换方法

//将旋转向量转化为旋转矩阵
Mat_<float> r_l = (Mat_<float>(3, 1) << 0.04345, -0.05236, -0.01810);//左摄像机的旋转向量
Mat_<float> r_r = (Mat_<float>(3, 1) << 0.04345, -0.05236, -0.01810);//右摄像机的旋转向量
Mat  R_R, R_L;
Rodrigues(r_l, R_L);
Rodrigues(r_r, R_R);

1
2
3
4
5
6
7

2、欧拉角和旋转矩阵之间的转化

以下是欧拉角转旋转矩阵

// 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;
}

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.
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);

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

三、总结

1、旋转向量

1）、旋转向量的初始化

初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
1

这里还有一种初始化的方式，上文也提到过（向量的模是旋转的角度，向量本身就是绕着旋转的轴）

Eigen::Vector3d w(0.01,0.02,0.03); // 这个模就是旋转的角度，这个本身就是旋转的轴
1

2）、旋转向量的一些转化

旋转向量两种初始化之间的转化

首先这里多了一个旋转向量他们两个初始化的方式如何转化的问题

 Eigen::AngleAxisd update_vector1(update_vector.norm(),Eigen::Vector3d(0.1/update_vector.norm(),0.2/update_vector.norm(),0.3/update_vector.norm()));
1

其实和下面是等同的

Eigen::Vector3d w(0.01,0.02,0.03);
double theta=w.norm();
Eigen::Vector3d n_w=w/theta;
Eigen::AngleAxisd w1 =(theta,n_w);
1
2
3
4

旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
1
2

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
1
2

如果是第二种初始化的方式转化旋转矩阵，就要用罗德里格斯公式了

Eigen::Vector3d w(0.01,0.02,0.03);//小量角速度（旋转向量）
double theta=w.norm();//旋转向量对应的旋转角
       Eigen::Vector3d n_w=w/theta;//归一化得到旋转向量对应的旋转轴
       Eigen::Matrix3d n_w_skew;
    n_w_skew<<   0,    -n_w(2),    n_w(1),
		      n_w(2),     0,     -n_w(0),
	         -n_w(1),  n_w(0),      0;
      Eigen::Matrix3d R_w=cos(theta)*Eigen::Matrix3d::Identity()+(1-cos(theta))*n_w*n_w.transpose()+sin(theta)*n_w_skew;//Rodrigues's formula
1
2
3
4
5
6
7
8

旋转向量转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
1

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);
1

Eigen::Quaterniond quaternion;
Quaterniond quaternion;
1
2

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;
1
2

2、旋转矩阵

1）、旋转矩阵的初始化

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
1
2

2）、旋转矩阵的一些转化

旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);
1

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;
1
2

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);
1
2

旋转矩阵转欧拉角

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);
1

旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);
1

Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;
1
2

3、欧拉角

1）欧拉角的初始化

Eigen::Vector3d eulerAngle(yaw,pitch,roll);
1

2）、欧拉角的一些转化

欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;
1
2
3
4
5
6

欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;
1
2
3
4
5
6

欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;
1
2
3
4
5
6

4、四元数

1）、四元数的初始化

Eigen::Quaterniond quaternion(w,x,y,z);
1

2）、四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);
1

Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;
1
2

四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
1
2

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
1
2

四元数转欧拉角

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
1