Eigen学习（八）几何模块的实践

    Eigen的几何模块用来表达空间的旋转、平移等变换。3维空间中刚体的运动有六个自由度，分别是绕3个轴的旋转运动和沿着3个轴的平移运动。对于旋转可以用3x3的旋转矩阵R表示，旋转矩阵描述了刚体经过矩阵作用后的姿态信息，旋转矩阵是一个正交矩阵；然而旋转矩阵有9个参数，9个参数描述6自由度的旋转，有点冗余了。因此旋转还可以用旋转向量来表示，空间中物体的旋转可以看作是绕这某个轴转过一定的角度完成，因此旋转矩阵是个3维向量，其方向代表转轴的方向，其大小代表旋转的角度。

    实际中物体不光有旋转，还有平移运动，如果用t表示平移向量，那么R*p+t可以描述刚体p的旋转加平移运动，然而当连续多次运动时整个表达式将会变得非常复杂，比如R1*(R*p+t)+t1描述连续两次的运动，因此为了简化书写形式引入齐次坐标的概念，将坐标扩充到4维，将旋转矩阵和平移向量写入一个4x4的变换矩阵中，简化了连续运动公式的形式，但是结果是16个参数描述一个6自由度的运动，更加冗余了。在旋转向量的后面增加3维代表平移向量，即用6维的旋转向量描述旋转和平移运动，看起来比较紧凑了，但是像欧拉角一样也会遇到万向锁问题，导致奇异性；最终即不冗余又紧凑又没有万向锁问题的解决方案是使用四元数描述旋转问题，这也是很多飞控代码中用到的方案。

    下面是对Eigen中的欧拉角、旋转向量、旋转矩阵、变换矩阵和四元数的练习代码

// Created by 开机烫手 on 2018/4/8.
#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <Eigen/Geometry>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;

int main() {

    // 旋转矩阵直接用Matrix3d即可
    Matrix3d rotation_matrix;
    rotation_matrix.setIdentity();
    // 旋转向量 由旋转轴和旋转角度组成
    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
    cout.precision(3);
    cout << "rotation vector: Angle is: " << rotation_vector.angle() * (180 / M_PI)
         << "  Axis is: " << rotation_vector.axis().transpose() << endl;
    cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 下面v是待旋转的向量，或者认为空间中的一个刚体的位置
    Vector3d v(1, 0, 0);
    Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    // 欧拉角 按ZYX的顺序 由旋转矩阵直接转换成欧拉角
    Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
    cout << "yaw pitch roll = " << euler_angles.transpose() * (180 / M_PI) << endl;

    // 变换矩阵  4x4的
    Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(rotation_vector);
//    T.rotate(rotation_matrix);    // 这样写也行，相当于由旋转矩阵构造变换矩阵
    // 设置平移向量
    T.pretranslate(Eigen::Vector3d(0, 0, 3));
    cout << "Transform matrix = \n" << T.matrix() << endl;

    // 用变换矩阵进行坐标变换
    Vector3d v_transformed = T * v;
    cout << "v transformed = " << v_transformed.transpose() << endl;

    // 由旋转向量构造四元数
    Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout << "quaternion = \n" << q.coeffs() << endl;
    // 由旋转矩阵构造四元数
    q = Eigen::Quaterniond(rotation_matrix);
    cout << "quaternion = \n" << q.coeffs() << endl;
    v_rotated = q * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    return 0;
}

输出为：

rotation vector: Angle is: 45  Axis is: 0 0 1
rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation = 0.707 0.707     0
(1,0,0) after rotation = 0.707 0.707     0
yaw pitch roll = 45 -0  0
Transform matrix =
 0.707 -0.707      0      0
 0.707  0.707      0      0
     0      0      1      3
     0      0      0      1
v transformed = 0.707 0.707     3
quaternion =
    0
    0
0.383
0.924
quaternion =
    0
    0
0.383
0.924
(1,0,0) after rotation = 0.707 0.707     0