0、关于
老师又想搞一个微型四旋翼…
这个得加入日程了,这两天和回家一起搞一下.
就一直拖到了现在,最近看代码发现Eigen在ROS用的是真的多,基于SE(3)的姿态结算也大行其道,十四讲内容还是从很基础的内容讲起来的,已经在整理了.
1、向量-矩阵变换
首先了解eigen基本的加减乘除后使用eigen进行姿态结算是十分方便的,不过有三个重要概念需要理解清楚:四元数 训传矩阵 欧拉角.
1
2
3
4
| // Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Matrix3d::Identity();//单位阵
cout << "rotation Identity =\n" << rotation_matrix.Identity() << endl;
|
1
2
3
4
5
6
| //旋转矩阵计算方法1
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
Eigen::AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //沿 Z 轴旋转 45 度
cout.precision(3); //输出结果取小数点后三位
cout << "rotation matrix1 =\n" << rotation_vector.matrix() << endl; //用matrix()转换成矩阵
|
1
2
3
| 0.707 -0.707 0
0.707 0.707 0
0 0 1
|
1
2
3
| //旋转矩阵计算方法2
rotation_matrix = rotation_vector.toRotationMatrix();
cout << "rotation toRotationMatrix =\n" << rotation_vector.toRotationMatrix() << endl; //用matrix()转换成矩阵
|
1
2
3
| 0.707 -0.707 0
0.707 0.707 0
0 0 1
|
1
2
3
4
5
6
7
8
9
10
|
// 坐标变换
//可以看做四元数中w=0 xyz=(1,0,0)
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rotated ;
v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
// 用旋转矩阵进行旋转之后得到的结果如下:
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated<< endl;
|
1
2
3
4
5
| // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles ;
euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
// 0.785 -0 0
|
1
2
3
4
5
6
7
8
9
10
11
12
| // 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Eigen::Quaterniond q;
q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
<< endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q * v; // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
|