slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用
文章目录
- 一、有关坐标系变换的一些概述
-
- 1、旋转向量
- 2、旋转矩阵
- 3、四元数
- 4、欧拉角
- 二、各种表示方法之间的转换
-
- 1、旋转向量、旋转矩阵、四元素之间的转化
- 2、欧拉角和旋转矩阵之间的转化
- 三、总结
一、有关坐标系变换的一些概述
坐标系的变换一般来说有四种方式表示变换：旋转向量、旋转矩阵、欧拉角和四元数，这里分别介绍下相应的原理及如何使用，最后附上相互转化的代码。
常用的一些如下：
旋转矩阵（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就是初始化的旋转向量。
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中的三次旋转都是正向旋转。
（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 2）这里opencv有罗德里格斯公式函数，完成旋转矩阵和旋转向量的互相转化的
#include 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);