从零开始学习Slam-旋转矩阵旋转向量四元组（二）

本文参考：计算机视觉life 仅作笔记用
书接上回，上回不清不楚的介绍了旋转矩阵&旋转向量和四元组
现在回顾一下重点：
本着绕谁谁不变的变则
假设绕z轴旋转θ，旋转矩阵为：
在这里插入图片描述
再回顾一下旋转向量的表示以及这个基本记不住的罗德里格斯公式，记不住也没事：

一个例子

已知旋转矩阵定义是沿着Z轴旋转45°。请按照该定义初始化旋转向量、旋转矩阵、四元数、欧拉角。请编程实现：

1、以上四种表达方式的相互转换关系并输出，并参考给出的结果验证是否正确。

2、假设平移向量为（1,2,3）,请输出旋转矩阵和该平移矩阵构成的欧式变换矩阵，并根据欧式变换矩阵提取旋转向量及平移向量。

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main() {
    // 初始化旋转参数
    double angle = M_PI/4; // 45°转换为弧度
    Vector3d axis = Vector3d::UnitZ(); // Z轴

    // 初始化旋转向量
    Vector3d rotation_vector = angle * axis;

    // 初始化旋转矩阵
    Matrix3d rotation_matrix;
    
    // AngleAxisd(angle, axis) 创建了一个表示绕着给定轴 axis 旋转 angle 弧度的旋转向量。
    //在Eigen库中，AngleAxisd 类表示一个旋转向量，它由一个旋转轴和一个旋转角度组成。
    //它的构造函数可以接受旋转角度和旋转轴作为参数，也可以接受一个旋转矩阵作为参数，并从中提取旋转轴和旋转角度。
    //调用 toRotationMatrix() 方法将该旋转向量转换为对应的旋转矩阵。
    rotation_matrix = AngleAxisd(angle, axis).toRotationMatrix();

    // 初始化四元数
    Quaterniond quaternion(rotation_matrix);

    // 初始化欧拉角
    Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序

    // 输出四种表达方式 
    // 旋转向量
    std::cout << "Rotation Vector: " << rotation_vector.transpose() << std::endl;  
    // .transpose()方法被用于将向量和矩阵输出成为行向量的形式，以方便显示在控制台上。
    // 旋转矩阵
    std::cout << "Rotation Matrix:\n" << rotation_matrix << std::endl;
    // coeffs()方法用于获取四元数对象的系数，返回一个四维向量。
    std::cout << "Quaternion: " << quaternion.coeffs()<< std::endl;
    // 欧拉角
    std::cout << "Euler Angles (ZYX): " << euler_angles.transpose() << std::endl;

    // 假设平移向量为（1,2,3）
    Vector3d translation_vector(1, 2, 3);

    // 构造欧式变换矩阵
    Matrix4d euclidean_transform_matrix = Matrix4d::Identity();  // 创建一个4x4的单位矩阵的静态方法
    // 将旋转矩阵赋值给欧几里德变换矩阵的左上角的3x3子矩阵，即旋转部分
    euclidean_transform_matrix.block<3,3>(0,0) = rotation_matrix;
    // 将平移向量赋值给欧几里德变换矩阵的右上角的3x1子矩阵，即平移部分
    euclidean_transform_matrix.block<3,1>(0,3) = translation_vector;

    /* 在Eigen库中，AngleAxisd()是用于创建一个旋转向量（Angle-Axis）的构造函数。Angle-Axis旋转表示通过一个旋转轴和一个旋转角度来描述旋转。具体来说，AngleAxisd()的使用方法如下
    
    Eigen::AngleAxisd angle_axis(angle, axis);
    
    在这段代码中，AngleAxisd(rotation_matrix)创建了一个AngleAxisd类型的对象，该对象代表了由旋转矩阵rotation_matrix表示的旋转。
    然后，.axis()方法用于获取该旋转向量的旋转轴，而.angle()方法用于获取旋转角度。
    接着，AngleAxisd(rotation_matrix).axis() * AngleAxisd(rotation_matrix).angle()部分将旋转向量的轴乘以旋转角度，这将得到一个旋转向量，其方向由旋转轴决定，大小由旋转角度决定。这个旋转向量被赋值给了extracted_rotation_vector。*/
    
    // 从欧式变换矩阵分解出旋转向量和平移向量
    Vector3d extracted_rotation_vector = AngleAxisd(rotation_matrix).axis() * AngleAxisd(rotation_matrix).angle();
    
    /* block<3,1>(0,3)是Eigen库中的一个函数，用于从矩阵中提取特定的块。在这里，block<3,1>(0,3)
    表示提取了从第0行开始，第3列开始的一个3x1的块，即欧氏变换矩阵的前三个元素（0, 1, 2行，3列），
    这通常是表示平移向量的部分。*/ 
    Vector3d extracted_translation_vector = euclidean_transform_matrix.block<3,1>(0,3);


    // 输出欧式变换矩阵、提取的旋转向量和平移向量
    
    // 欧式变换矩阵
    std::cout << "\nEuclidean Transformation Matrix:\n" << euclidean_transform_matrix << std::endl;
    // 旋转向量
    std::cout << "Extracted Rotation Vector: " << extracted_rotation_vector.transpose() << std::endl;
   // 平移向量
    std::cout << "Extracted Translation Vector: " << extracted_translation_vector.transpose() << std::endl;

    return 0;
}