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

文章目录

  • 一、有关坐标系变换的一些概述
    • 1、旋转向量
    • 2、旋转矩阵
    • 3、四元数
    • 4、欧拉角
  • 二、各种表示方法之间的转换
    • 1、旋转向量、旋转矩阵、四元素之间的转化
    • 2、欧拉角和旋转矩阵之间的转化
  • 三、总结

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

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

常用的一些如下:

旋转矩阵(3X3):Eigen::Matrix3d

旋转向量(3X1):Eigen::AngleAxisd

四元数(4X1):Eigen::Quaterniond

平移向量(3X1):Eigen::Vector3d

变换矩阵(4X4):Eigen::Isometry3d

slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_第1张图片

1、旋转向量

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

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

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

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

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

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

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

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

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

2、旋转矩阵

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

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

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

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;

3、四元数

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

1)方式一:实部为1,虚部为2,3,4.

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

2)方式二:第二种方式 实部为4 ,虚部1,2,3

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

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中的三次旋转都是正向旋转。

slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_第2张图片

(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)内旋和外旋:

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

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

下图说明了内旋和外旋的区别。
slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_第3张图片
slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_第4张图片

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

slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_第5张图片

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

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

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

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

#include 
#include 


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;


}

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

slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_第6张图片

#include 
#include 
 
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]);
    }
}

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

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

以下是旋转矩阵转化欧拉角

// 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、旋转向量

1)、旋转向量的初始化

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

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

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

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

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

其实和下面是等同的

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

旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

如果是第二种初始化的方式转化旋转矩阵,就要用罗德里格斯公式了
在这里插入图片描述

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

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

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

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;
Quaterniond quaternion;
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

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;

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

旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

旋转矩阵转欧拉角

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

旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

3、欧拉角

1)欧拉角的初始化

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

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;

欧拉角转旋转矩阵

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;

欧拉角转四元数

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;

4、四元数

1)、四元数的初始化

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

2)、四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

四元数转欧拉角

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

你可能感兴趣的:(slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用)