4、数学基础（下）：齐次坐标与变换矩阵（4x4），刚体变换的链式法则

上一讲我们把旋转矩阵、平移向量这些基础概念捋了一遍。说实话，那些东西单独看都不难，但一旦要把它们组合起来描述一个完整的刚体运动，就会遇到一个很烦人的问题——旋转是乘法，平移是加法，混在一起写公式特别容易出错。

这一讲我们就来解决这个问题。核心工具就是齐次坐标和4x4变换矩阵。我个人觉得，这是整个多传感器融合里最优雅的一个数学设计，没有之一。

4.1 为什么需要齐次坐标？

先看一个简单的场景：一个点 P 在世界坐标系下的坐标是 (x, y, z)，它先绕 Z 轴旋转了 30°，再沿着 X 轴平移了 5 米。用我们之前学的公式，你得写成：

P' = R * P + t

这看起来还行对吧？但如果你有连续多个变换呢？比如：

P''' = R3 * (R2 * (R1 * P + t1) + t2) + t3

嗯，括号套括号，写起来累，读起来更累。而且你没法把多个变换“合并”成一个简洁的表达式。

齐次坐标的核心思想：给三维向量多加一个维度，把旋转和平移统一成一次矩阵乘法。

具体做法很简单——把一个三维点 (x, y, z) 表示成四维齐次坐标 (x, y, z, 1)。多出来的那个 1 就是“缩放因子”，我们一般固定为 1。

齐次坐标 vs 欧氏坐标

类型	表示方式	变换操作	优点
欧氏坐标	(x, y, z)	旋转用乘法，平移用加法	直观，占用内存少
齐次坐标	(x, y, z, 1)	旋转和平移统一为矩阵乘法	可合并、可求逆、可链式

4.2 4x4 变换矩阵的结构

一个标准的 4x4 变换矩阵长这样：

| R11  R12  R13  tx |
| R21  R22  R23  ty |
| R31  R32  R33  tz |
|  0    0    0    1  |

左上角的 3x3 是旋转矩阵 R，右上角的 3x1 是平移向量 t，最后一行固定是 [0, 0, 0, 1]。

用这个矩阵去乘一个齐次坐标点，效果等价于先旋转再平移：

| R  t |   | x |   | R*x + t |
| 0  1 | * | y | = |   ...   |
            | z |
            | 1 |

你看，一次矩阵乘法就搞定了旋转和平移。我刚开始做融合的时候，手动推导公式经常搞混旋转和平移的顺序，自从用了齐次矩阵，再也没出过这种低级错误。

我的习惯：在代码里，我一般用 Eigen 库的 Isometry3d 或 Affine3d 来管理变换矩阵。Eigen 会自动帮你处理齐次坐标的转换，你只需要关心旋转和平移的数值。

4.3 刚体变换的链式法则

这才是实战中的重头戏。在自动驾驶里，一个激光雷达点要经过好几次坐标变换才能投影到图像上：

激光雷达坐标系 → 车辆本体坐标系
车辆本体坐标系 → 惯性导航坐标系
惯性导航坐标系 → 世界坐标系
世界坐标系 → 相机坐标系
相机坐标系 → 像素坐标系

如果每个变换都用独立的旋转矩阵和平移向量去算，代码会变得又臭又长。但用齐次变换矩阵，你可以把它们乘在一起：

T_total = T_5 * T_4 * T_3 * T_2 * T_1

然后直接用这个 T_total 去变换原始点：

P_pixel = T_total * P_lidar

这就是链式法则——多个刚体变换的复合，等价于它们对应的齐次变换矩阵按顺序相乘。

我曾经踩过的坑：矩阵乘法不满足交换律！T1 * T2 和 T2 * T1 的结果完全不同。在链式法则里，变换的顺序必须严格遵循坐标系之间的层级关系。我见过有人把外参矩阵和内参矩阵乘反了，结果点云投影到图像上全跑到了左上角，排查了半天才发现是顺序问题。

4.4 逆变换与求逆

有时候我们需要求一个变换的逆——比如已知车辆到激光雷达的变换，想求激光雷达到车辆的变换。对于齐次变换矩阵，求逆有很简洁的公式：

T = | R  t |
    | 0  1 |

T^(-1) = | R^T  -R^T * t |
         |  0      1      |

注意，这里利用了旋转矩阵的逆等于它的转置这个性质。平移部分不是简单的取负号，而是要把旋转的影响也考虑进去。

我个人习惯在代码里直接用 Eigen 的 .inverse() 方法，但理解这个公式能帮你快速判断计算结果是否合理。

4.5 知识体系总览

下面这张图把这一讲的核心逻辑串起来了：

4.6 代码实战：用 Eigen 实现链式变换

光说不练假把式。我们直接写一段代码，把刚才的理论落地：

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

int main() {
    // 定义两个变换矩阵
    // T1: 绕 Z 轴旋转 30° 并平移 (1, 2, 3)
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    T1.rotate(Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitZ()));
    T1.pretranslate(Eigen::Vector3d(1, 2, 3));
    
    // T2: 绕 X 轴旋转 45° 并平移 (4, 5, 6)
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.rotate(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitX()));
    T2.pretranslate(Eigen::Vector3d(4, 5, 6));
    
    // 链式法则：先 T1 再 T2
    Eigen::Isometry3d T_total = T2 * T1;
    
    // 原始点 (1, 0, 0)
    Eigen::Vector4d p(1, 0, 0, 1);
    
    // 应用变换
    Eigen::Vector4d p_transformed = T_total.matrix() * p;
    
    std::cout << "变换后的点: " << p_transformed.transpose() << std::endl;
    
    // 求逆变换
    Eigen::Isometry3d T_inv = T_total.inverse();
    Eigen::Vector4d p_back = T_inv.matrix() * p_transformed;
    
    std::cout << "逆变换回原点: " << p_back.transpose() << std::endl;
    
    return 0;
}

这段代码里，Isometry3d 是 Eigen 专门为刚体变换设计的类型，它内部维护的就是一个 4x4 齐次变换矩阵。你想想看，如果不用齐次坐标，你要手动写多少行代码才能实现同样的功能？

一个小技巧：在调试时，我经常把变换矩阵打印出来，检查最后一行是不是 [0, 0, 0, 1]。如果发现最后一行变了，说明某个地方不小心做了仿射变换或投影变换，而不是刚体变换。这在传感器融合里通常意味着代码有 bug。

4.7 总结

齐次坐标和 4x4 变换矩阵，说白了就是把旋转和平移打包成一个整体。它的好处在链式法则里体现得最明显——多个坐标系之间的变换可以合并成一个矩阵，代码简洁，逻辑清晰，不容易出错。

📚 课程目录

4、数学基础（下）：齐次坐标与变换矩阵（4x4），刚体变换的链式法则

4.1 为什么需要齐次坐标？

4.2 4x4 变换矩阵的结构

4.3 刚体变换的链式法则

4.4 逆变换与求逆

4.5 知识体系总览

4.6 代码实战：用 Eigen 实现链式变换

4.7 总结