矩阵变换基础与C++实现：从理论到实战（一）

矩阵变换基础与C++实现：从理论到实战

矩阵变换是计算机图形学、机器人学和游戏开发中的核心概念。无论是3D模型的旋转、平移，还是传感器数据的坐标转换，矩阵都扮演着关键角色。本文将通过C++代码示例，详细讲解矩阵基础、旋转矩阵、齐次坐标变换，以及四元数与欧拉角的对比。

---

一、矩阵基础与运算

1.1 矩阵的定义

矩阵是由数值排列成的矩形阵列，常用于表示线性变换。例如，一个3x3的矩阵可以表示三维空间中的旋转变换：

#include <Eigen/Dense>

// 定义一个3x3矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix << 1, 0, 0,
                   0, 1, 0,
                   0, 0, 1; // 单位矩阵

1.2 矩阵运算

• 矩阵加法：对应元素相加。
• 矩阵乘法：行乘列求和。

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 C = A + B; // 加法
Eigen::Matrix3d D = A * B; // 乘法

---

二、二维旋转矩阵

二维旋转矩阵用于将点绕原点旋转θ角。其形式为：
$$R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$
C++实现：

#include <cmath>

Eigen::Matrix2d create2DRotationMatrix(double theta) {
    Eigen::Matrix2d R;
    R << cos(theta), -sin(theta),
         sin(theta), cos(theta);
    return R;
}

// 示例：旋转90度
Eigen::Vector2d point(1, 0);
Eigen::Matrix2d R = create2DRotationMatrix(M_PI / 2);
Eigen::Vector2d rotated_point = R * point; // 结果：(0, 1)

---

三、三维旋转矩阵

三维旋转分为绕X、Y、Z轴的旋转，矩阵形式如下：

3.1 绕X轴旋转

$$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$

3.2 绕Y轴旋转

$$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

3.3 绕Z轴旋转

$$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

C++实现绕Y轴旋转90度：

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()).toRotationMatrix();
Eigen::Vector3d point(1, 0, 0);
Eigen::Vector3d new_point = R * point; // 结果：(0, 0, -1)

---

四、齐次坐标与变换矩阵

齐次坐标通过4x4矩阵将旋转和平移统一表示，结构如下：
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$

4.1 齐次矩阵的构造

Eigen::Matrix4d createHomogeneousMatrix(const Eigen::Matrix3d& rotation, 
                                        const Eigen::Vector3d& translation) {
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3, 3>(0, 0) = rotation;
    T.block<3, 1>(0, 3) = translation;
    return T;
}

// 示例：绕Z轴旋转90度并平移(1, 2, 3)
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()).toRotationMatrix();
Eigen::Matrix4d T = createHomogeneousMatrix(R, Eigen::Vector3d(1, 2, 3));

4.2 坐标变换实战

以下示例将点从 ​局部坐标系（Local Frame）​​ 转换到 ​世界坐标系（World Frame）​。

4.2.1 输入数据定义

#include <Eigen/Dense>

// 定义位姿结构体（替代私有消息类型）
struct Pose {
    Eigen::Quaterniond rotation;  // 局部坐标系到世界坐标系的旋转
    Eigen::Vector3d position;     // 局部坐标系原点在世界坐标系中的位置
};

// 待转换的点（局部坐标系下的坐标）
Eigen::Vector3d local_point(1.0, 0.0, 0.0);

4.2.2 核心转换函数

Eigen::Vector3d convertLocalToWorld(const Eigen::Vector3d& local_point, const Pose& pose) {
    // 步骤1：构造齐次变换矩阵（World → Local）
    Eigen::Matrix4d T_local_to_world = Eigen::Matrix4d::Identity();
    T_local_to_world.block<3, 3>(0, 0) = pose.rotation.toRotationMatrix();
    T_local_to_world.block<3, 1>(0, 3) = pose.position;

    // 步骤2：求逆矩阵（Local → World）
    Eigen::Matrix4d T_world_to_local = T_local_to_world.inverse();

    // 步骤3：提取旋转和平移
    Eigen::Matrix3d R = T_world_to_local.block<3, 3>(0, 0);
    Eigen::Vector3d t = T_world_to_local.block<3, 1>(0, 3);

    // 步骤4：应用变换
    Eigen::Vector3d world_point = R * local_point + t;
    return world_point;
}

4.2.3 代码解释

1. 齐次矩阵构造：

   • T_local_to_world 表示从世界坐标系到局部坐标系的变换。
   • block<3, 3>(0, 0) 填充旋转矩阵，block<3, 1>(0, 3) 填充平移向量。

2. 逆矩阵计算：

   • T_world_to_local = T_local_to_world.inverse() 得到从局部坐标系到世界坐标系的变换。

3. ​坐标变换公式：

   • 公式：
     $$P_{\text{world}}=R\cdot P_{\text{local}}+t$$

---

五、四元数与欧拉角对比

特性	欧拉角	四元数
直观性	高（三个角度）	低（四维参数）
万向节锁	存在	不存在
插值	不自然	平滑（SLERP）
计算效率	低	高

四元数与欧拉角转换：

// 欧拉角转四元数
Eigen::Quaterniond eulerToQuaternion(double yaw, double pitch, double roll) {
    return Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
}

// 四元数转欧拉角
Eigen::Vector3d quaternionToEuler(const Eigen::Quaterniond& q) {
    return q.toRotationMatrix().eulerAngles(2, 1, 0); // Z-Y-X顺序
}

---

六、总结

矩阵变换是3D图形和机器人学的基石。通过齐次坐标和四元数，开发者可以高效处理复杂的空间变换。本文代码示例基于Eigen库，读者可结合实际项目进一步探索。

欢迎在评论区交流讨论！
关注我的优快云，获取更多技术干货！

---

标签：矩阵变换、C++、机器人学、计算机图形学、四元数