1.2Eigen库的几何应用

 本文学习来源主要基于高翔博士—slam14讲

目录

1、表示旋转的几个方法

1.1 旋转矩阵

1.2旋转向量（或轴角，AxisAngle）

1.3欧拉角

1.4四元数

1.5变换矩阵（旋转矩阵加上平移量,所以并不是一种新的表示旋转的方法）

2、各个表示旋转方式之间的转换

2.1 旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )

2.2旋转向量与四元数

2.3旋转矩阵与四元数

3、代码实操

---

1、表示旋转的几个方法

1.1 旋转矩阵

它是一个行列式为 1 的正交矩阵

1.2旋转向量（或轴角，AxisAngle）

一个旋转轴为 n，角度为 θ 的旋转，它对应的旋转向 量为 θn。

1.3欧拉角

绕物体的 Z 轴旋转，得到偏航角 yaw；
绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
绕旋转之后的 X 轴旋转，得到滚转角 roll。

备注：只用于人机交互中；    
不会在程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋

1.4四元数

i, j, k 为四元数的三个虚部

1.5变换矩阵（旋转矩阵加上平移量,所以并不是一种新的表示旋转的方法）

 两次变换（旋转+平移=变换）如果按如下方式表达式会显得过于复

 如果一次变换按如下表示：

我们把一个三维向量的末尾添加 1，变成了四维向量，称为齐次 坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成 了线性关系。该式中，矩阵 T 称为变换矩阵（Transform Matrix）

而两次（或者多次变换）可以写成以下方式（看起来简介易懂）

2、各个表示旋转方式之间的转换

2.1 旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )

2.2旋转向量与四元数

2.3旋转矩阵与四元数

假设矩阵为 R = {mij}, i, j ∈ [1, 2, 3]，其对 应的四元数 q 由下式给出： 

3、代码实操

//基于高翔博士的代码，增加了一点注释

#include <iostream>
#include <cmath>

using namespace std;

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法

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

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f    Identity()表示3*3单位矩阵
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
 
 cout.precision(3);


  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 也可以直接赋值//旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )变换
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 AngleAxis 可以进行坐标变换
  Vector3d v(1, 0, 0);
  Vector3d 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.transpose() << endl;

  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角  Eigen::vector3d eulerAngles(Index a0, Index a1, Index a2)，其中参数a0 a1 a2 用 0 1 2表示旋转轴，其中0表示X轴，1表示Y轴，2表示Z轴。
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序   //此外，a0 表示首先选择的轴，a1 表示其次旋转的轴，a2 表示最后旋转的轴。输出结果的顺序与轴旋转的顺序相同。   Pi/4 = 0.785,  即沿着z轴旋转45度

  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                            // 按照rotation_vector进行旋转   0.707 0.707  0
  T.pretranslate(Vector3d(1, 3, 4));                    // 把平移向量设成(1,3,4)
//T（0，3）=1； T（1，3）=3；  T（2，3）=4；
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // 用变换矩阵进行坐标变换做了什么变换：把（1，0，0）做了沿 Z 轴旋转 45 度和平移（1，3，4）
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

  // 四元数
  // 可以直接把AngleAxis赋值给四元数，反之亦然？？？coeffs是什么
  Quaterniond 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;

/四元数转换成旋转矩阵
//Eigen::Matrix3d qm = q.toRotationMatrix();

  // 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是qvq^{-1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}

 以下是终端输出的代码结果。