从零开始学习SLAM（三）

旋转向量

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

AngleAxisd 类有几种构造函数，其中最常用的是：

Eigen::AngleAxisd(const Scalar& angle, const Axis& axis);

angle 是旋转的角度，通常以弧度表示。
axis 是旋转的轴，通常是一个单位向量，表示旋转的方向。

例如，要创建一个围绕 Z 轴顺时针旋转 π/4 弧度（45度）的 AngleAxisd 对象，可以这样写：

Eigen::AngleAxisd rotation_angle_axis(M_PI / 4.0, Eigen::Vector3d::UnitZ());

或者是

Eigen::AngleAxisd rotation_angle_axis(M_PI / 4.0, Eigen::Vector3d(::UnitZ(0,0,1));

使用

使用 AngleAxisd 对象可以执行以下操作：

将其应用于向量或点进行旋转：

// 通过将 rotation_angle_axis 应用到 point 上得到的旋转后的新点
Eigen::Vector3d point(1.0, 0.0, 0.0);
Eigen::Vector3d rotated_point = rotation_angle_axis * point;

转换为旋转矩阵或四元数：

// 通过 toRotationMatrix() 方法从 rotation_angle_axis 转换而来，用于表示相同旋转的旋转矩阵
Eigen::Matrix3d rotation_matrix = rotation_angle_axis.toRotationMatrix();
// 通过直接将 rotation_angle_axis 赋值给 Eigen::Quaterniond 类型得到，也表示了同样旋转的四元数
Eigen::Quaterniond quaternion = rotation_angle_axis;

进行组合和插值操作：

// rotation_angle_axis 和 another_rotation 相乘（组合）得到的新的 Eigen::AngleAxisd 对象，它代表了先进行 rotation_angle_axis 的旋转，然后进行 another_rotation 的旋转。
Eigen::AngleAxisd another_rotation(M_PI / 3.0, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd combined_rotation = rotation_angle_axis * another_rotation;
// interpolated_rotation 是通过 slerp() 方法进行球面线性插值（Slerp）得到的 Eigen::AngleAxisd 对象。这种方法可以在两个旋转之间进行平滑的过渡，第一个参数是插值参数，通常是一个介于0到1之间的值，表示两个旋转的相对比例。
Eigen::AngleAxisd interpolated_rotation = rotation_angle_axis.slerp(0.5, another_rotation);

欧氏变换

Eigen::Isometry3d 是 Eigen 库中用于表示三维欧氏空间中的等距变换（Isometry）的类。它继承自 Eigen::Transform，具体表示了包括平移和旋转在内的等距变换。

主要特点和用途：

Identity Isometry（单位等距变换）：单位等距变换表示没有任何旋转或平移，即一个点经过单位等距变换后位置不变。在三维空间中，单位等距变换的旋转部分是单位矩阵，平移部分是零向量。
静态成员函数 Identity()：这个函数是通过 Isometry3d 类访问的静态函数，它返回一个默认的单位等距变换对象。通过调用 Eigen::Isometry3d::Identity()，可以获得一个已经初始化为单位变换的 Isometry3d 对象，通常用于开始定义其他具体的变换。

等距变换（Isometry）：Isometry3d 能够表示旋转和平移的组合，保持点之间的距离不变。在计算机图形学、机器人学和几何计算等领域中，等距变换非常重要。
旋转和平移的组合：通过 Isometry3d，可以方便地表示和操作三维空间中的物体的姿态和位置。
Eigen 库支持：Eigen 是一个开源的线性代数库，专注于高性能的矩阵和向量运算。Isometry3d 类充分利用了 Eigen 的矩阵和向量运算优势，提供了高效的数学运算。

示例：

以下是一个简单的示例，展示如何使用 Isometry3d 类创建和操作等距变换：

#include <Eigen/Geometry>

int main() {
    // 创建一个 Isometry3d 对象
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    // 设置旋转部分（绕Z轴旋转90度）
    T.rotate(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()));

    // 设置平移部分（平移向量）
    T.pretranslate(Eigen::Vector3d(1, 2, 3));

    // 使用 Isometry3d 进行点变换
    Eigen::Vector3d point(1, 0, 0);
    Eigen::Vector3d transformed_point = T * point;

    // 输出变换后的点
    std::cout << "Transformed point: " << transformed_point.transpose() << std::endl;

    return 0;
}

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

int main() {
    // 假设我们有一个4x4的仿射变换矩阵
    Eigen::Affine3d T;
    T.matrix() << 
        1, 0, 0, 1, // 旋转矩阵部分
        0, 1, 0, 0, // 旋转矩阵部分
        0, 0, 1, 0, // 旋转矩阵部分
        1, 2, 3, 1; // 平移向量

    // 提取旋转矩阵
    Eigen::Matrix3d R = T.linear();
    
    // 提取平移向量
    Eigen::Vector3d t = T.translation();
    
    // 打印旋转矩阵和平移向量
    std::cout << "Rotation matrix:\n" << R << std::endl;
    std::cout << "Translation vector:\n" << t.transpose() << std::endl;
    
    return 0;

}

T.linear()

这个成员函数返回仿射变换中的线性部分，即旋转矩阵。它返回的是一个Eigen::Matrix3d类型的引用，代表3x3的旋转矩阵。

T.translation()

这个成员函数返回仿射变换中的平移向量。它返回的是一个Eigen::Vector3d类型的引用，代表3D空间中的平移。

T.rotate()

这个成员函数是用来设置仿射变换的旋转部分的。你可以传递一个3x3的旋转矩阵给这个函数，它会更新T的线性部分。

旋转矩阵

Eigen::Matrix3d 在实际应用中有许多用途，以下是几个常见的例子：

1. 旋转矩阵表示旋转操作

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

int main() {
    // 定义一个旋转矩阵，将向量绕Z轴旋转45度
    Eigen::Matrix3d rotation_matrix;
    double angle = M_PI / 4.0; // 45度
    rotation_matrix << cos(angle), -sin(angle), 0,
                       sin(angle), cos(angle), 0,
                       0, 0, 1;

    // 定义一个向量
    Eigen::Vector3d vector(1.0, 0.0, 0.0);

    // 将向量应用旋转
    Eigen::Vector3d rotated_vector = rotation_matrix * vector;

    // 输出结果
    std::cout << "Original vector: " << vector.transpose() << std::endl;
    std::cout << "Rotated vector: " << rotated_vector.transpose() << std::endl;

    return 0;
}

这个例子中，我们定义了一个 Eigen::Matrix3d 类型的 rotation_matrix，表示绕Z轴旋转45度的旋转矩阵。然后，我们定义了一个 Eigen::Vector3d 类型的向量 vector，并将其通过 rotation_matrix 进行旋转操作，得到 rotated_vector。最后输出了旋转前后的向量。

2. 坐标变换

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

int main() {
    // 定义一个坐标变换矩阵，将点从局部坐标系变换到全局坐标系
    Eigen::Matrix3d coordinate_transform;
    coordinate_transform << 1, 0, 0,
                            0, -1, 0,
                            0, 0, 1;

    // 定义一个局部坐标系下的点
    Eigen::Vector3d local_point(2.0, 3.0, 1.0);

    // 应用坐标变换
    Eigen::Vector3d global_point = coordinate_transform * local_point;

    // 输出结果
    std::cout << "Local point: " << local_point.transpose() << std::endl;
    std::cout << "Global point: " << global_point.transpose() << std::endl;

    return 0;
}

在这个例子中，我们定义了一个 Eigen::Matrix3d 类型的 coordinate_transform，表示一个坐标系的变换矩阵，用来将局部坐标系下的点 local_point 转换到全局坐标系下的 global_point。这种方式在计算机图形学和仿真中经常使用，用于物体的位置和姿态变换。

3. 矩阵运算

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

int main() {
    // 定义两个矩阵
    Eigen::Matrix3d A, B;
    A << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    B << 9, 8, 7,
         6, 5, 4,
         3, 2, 1;

    // 计算矩阵乘法
    Eigen::Matrix3d result = A * B;

    // 输出结果
    std::cout << "Matrix A:\n" << A << std::endl;
    std::cout << "Matrix B:\n" << B << std::endl;
    std::cout << "Result of A * B:\n" << result << std::endl;

    return 0;
}

当使用Eigen库进行旋转表示的转换时，可以如下操作：

1. 旋转矩阵转换为旋转向量

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

int main() {
    // 定义一个旋转矩阵，例如绕Y轴旋转30度
    Eigen::Matrix3d rotation_matrix;
    double angle = M_PI / 6.0; // 30度
    rotation_matrix << cos(angle), 0, sin(angle),
                       0, 1, 0,
                       -sin(angle), 0, cos(angle);

    // 将旋转矩阵转换为旋转向量
    Eigen::AngleAxisd rotation_vector(rotation_matrix);

    // 输出结果
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;
    std::cout << "Equivalent rotation vector:\n" << rotation_vector.axis().transpose()
              << " " << rotation_vector.angle() << " radians" << std::endl;

    return 0;
}

在这个例子中，我们定义了一个旋转矩阵 rotation_matrix，表示绕Y轴旋转30度。然后，使用 Eigen::AngleAxisd 类型的构造函数将旋转矩阵转换为对应的旋转向量 rotation_vector。通过 rotation_vector.axis() 获取旋转向量的轴向量，通过 rotation_vector.angle() 获取旋转角度。

2. 旋转矩阵转换为四元数

Eigen::Matrix3d rotation_matrix;
// 填充旋转矩阵

Eigen::Quaterniond quaternion(rotation_matrix);

这段代码示例已经在前面提供过，展示了如何将旋转矩阵 rotation_matrix 转换为对应的四元数 quaternion。

3. 旋转矩阵转换为欧拉角

Eigen库中没有直接提供将旋转矩阵转换为欧拉角的函数，但可以通过以下步骤手动实现欧拉角的计算：

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

int main() {
    // 定义一个旋转矩阵，例如绕Z轴旋转60度
    Eigen::Matrix3d rotation_matrix;
    double angle = M_PI / 3.0; // 60度
    rotation_matrix << cos(angle), -sin(angle), 0,
                       sin(angle), cos(angle), 0,
                       0, 0, 1;

    // 计算欧拉角（ZYX顺序）
    double phi = atan2(rotation_matrix(1, 0), rotation_matrix(0, 0));
    double theta = atan2(-rotation_matrix(2, 0), sqrt(rotation_matrix(2, 1) * rotation_matrix(2, 1) + rotation_matrix(2, 2) * rotation_matrix(2, 2)));
    double psi = atan2(rotation_matrix(2, 1), rotation_matrix(2, 2));

    // 输出结果
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;
    std::cout << "Equivalent Euler angles (ZYX order):\n"
              << "Phi (roll): " << phi << " radians\n"
              << "Theta (pitch): " << theta << " radians\n"
              << "Psi (yaw): " << psi << " radians" << std::endl;

    return 0;
}

在这个例子中，我们定义了一个旋转矩阵 rotation_matrix，表示绕Z轴旋转60度。然后，通过手动计算欧拉角（ZYX顺序），即 roll (phi), pitch (theta), yaw (psi)，从旋转矩阵中提取这些角度信息。

这些例子展示了如何使用 Eigen 库进行旋转表示的各种转换，从而在不同的场景中方便地处理旋转操作。

四元数

Eigen::Quateriond

Eigen::Quaterniond 是 Eigen 库中用于表示双精度（double）的四元数的类。四元数在计算机图形学和机器人学等领域中广泛用于表示旋转。让我展示如何使用 Eigen::Quaterniond 类来创建和操作四元数：

创建一个四元数

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

int main() {
    // 创建一个四元数，表示绕Z轴旋转45度
    Eigen::Quaterniond quaternion;
    double angle = M_PI / 4.0; // 45度
    quaternion = Eigen::Quaterniond(cos(angle / 2), 0, 0, sin(angle / 2));

    // 输出四元数的信息
    std::cout << "Quaternion:\n" << quaternion.coeffs().transpose() << std::endl;

    return 0;
}

在这个例子中，我们创建了一个四元数 quaternion，它表示绕Z轴旋转45度。四元数的构造方法 Eigen::Quaterniond 接受四个参数 (w, x, y, z)，分别表示四元数的实部和虚部（三个虚部分量）。在这里，我们使用角度 angle 计算实部 w 和虚部 z。

操作四元数

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

int main() {
    // 创建一个四元数，表示绕Z轴旋转45度
    Eigen::Quaterniond quaternion(cos(M_PI / 8), 0, 0, sin(M_PI / 8));

    // 获取旋转矩阵
    Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();

    // 输出旋转矩阵
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;

    return 0;
}

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

int main() {
    // 创建一个四元数，表示绕Z轴旋转45度
    Eigen::Quaterniond quaternion(cos(M_PI / 8), 0, 0, sin(M_PI / 8));

    // 获取旋转矩阵
    Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();

    // 输出旋转矩阵
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;

    return 0;
}