SLAM第二讲：旋转向量和欧拉角

1. 介绍

旋转矩阵（3 × 3）：Eigen::Matrix3d

旋转向量（3 × 1）：Eigen::AngleAxisd

欧拉角（3 × 1）：Eigen::Vector3d

四元数（4 × 1）：Eigen::Quaterniond

欧式变换矩阵（4 × 4）：Eigen::Isometry3d

仿射变换（4 × 4）：Eigen::Affine3d

射影变换（4 × 4）：Eigen::Projective3d

2. 代码

#include <iostream>
#include <ctime>

using namespace std;

// Eigen 部分
#include <eigen3/Eigen/Core>
// Eigen 几何部分
#include <eigen3/Eigen/Geometry>

/**
 * 演示 Eigen 几何模块的使用
 */

int main(int argc, char** argv)
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f 类表示
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis，它底层不直接是 Matrix,但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1)); // 绕 Z 轴旋转 45 度
    cout .precision(3); // 设置精度为 3 位
    cout << "Rotation vector:\n" << rotation_vector.matrix() << endl;  // 用 matrix() 转化为矩阵

    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 也可以反向转换
    Eigen::Vector3d v (1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation:\n" << v_rotated.transpose() << endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation:\n" << v_rotated.transpose() << endl;

    // 欧拉角：可以将旋转矩阵转换为欧拉角，也可以将欧拉角转换为旋转矩阵
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // 2,1,0 表示 ZYX 顺序, 即 yaw-pitch-roll
    cout << "yaw-pitch-roll angles:\n" << euler_angles.transpose() << endl;

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

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_trandformed = T * v; // 相当于 R * v + t
    cout << "v transformed:\n" << v_trandformed.transpose() << endl;

    // 对于仿射和射影变换，可以用 Eigen::Affine3d 和 Eigen::Projective3d 类进行表示

    // 四元数：Eigen::Quaterniond 类表示四元数，可以用其进行旋转、平移、缩放等变换
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    // coeffs 的顺序是 x,y,z,w, w 代表实部，其他三个分量代表虚部
    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:\n" << v_rotated.transpose() << endl;

    return 0;
}