文章目录
一、有关坐标系变换的一些概述
坐标系的变换一般来说有四种方式表示变换：旋转向量、旋转矩阵、欧拉角和四元数，这里分别介绍下相应的原理及如何使用，最后附上相互转化的代码。
常用的一些如下：
旋转矩阵（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 <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;
}
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]);
}
}
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);