slam十四讲 03 Eigen实践之三维空间刚体运动

目录

1 初始化

2 旋转空间中的向量

3 欧拉角

4 变换矩阵

5 四元素

完整程序

---

1 初始化

旋转的两种办法：

（1）旋转矩阵：a = Ra', a' = R^T a, 旋转矩阵的特性：是一个行列式为1的正交矩阵. 三维空间的旋转是3x3矩阵，即9个变量控制。

（2）旋转向量：方向与旋转轴一致，长度等于旋转角度，及只需要一个三维向量即可描述旋转。

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 1, 初始化旋转向量使用 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()转换成矩阵
  // 2, 初始化旋转矩阵，也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();

 rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1

2 旋转空间中的向量

  // 3, 旋转，用 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;

  // 4, 旋转，或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0 

3 欧拉角

无论是旋转矩阵还是旋转向量，对人类来说都不直观，欧拉角则把一个旋转分解成3次绕不同轴的旋转。3个轴上的转角。

由旋转矩阵->欧拉角

  // 5, 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll(ypr)顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

yaw pitch roll = 0.785    -0     0

4 变换矩阵

旋转加平移

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

  // 7, 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0            1      4
     0      0            0      1 

v tranformed = 1.71 3.71    4 

5 四元素

旋转矩阵用9个量描述3个自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但是具有奇异性。四元素既是紧凑的，也没有奇异性。缺点是不够直观，其运算稍复杂。

q是四元素，指定旋转。如果需要平移，则在公式后面+t即可。

  // 8, 四元数
  // 初始化1，可以直接把选择向量AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;   
  // 初始化2，也可以把旋转矩阵赋给它
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;

quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924 

  // 使用四元数旋转一个向量，使用重载的乘法即可
  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;

(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0 

完整程序

#include 
#include 

using namespace std;

#include 
#include 

using namespace Eigen;

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

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

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 1, 初始化旋转向量使用 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()转换成矩阵
  // 2, 初始化旋转矩阵，也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();

  // 3, 旋转，用 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;

  // 4, 旋转，或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // 5, 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll(ypr)顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

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

  // 7, 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

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

  // 8, 四元数
  // 初始化1，可以直接把选择向量AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;   
  // 初始化2，也可以把旋转矩阵赋给它
  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;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}