第三节：刚体变换与齐次坐标

各位好，我是老张。今天咱们聊聊刚体变换和齐次坐标。说实话，这个知识点在感知联调里太常用了，但也是坑最多的地方之一。

我记得刚入行那会儿，总觉得坐标变换不就是乘个矩阵嘛，有啥难的？结果第一次做激光雷达和相机的联合标定，就栽了个大跟头。嗯，从那以后，我对这块就格外上心了。

3.1 刚体变换：平移+旋转

先说说什么是刚体变换。说白了，就是一个物体在空间里移动，形状和大小都不变。你想想看，一辆车在路上开，它本身不会变形吧？这就是刚体变换。

刚体变换由两部分组成：

旋转：物体绕某个轴转动
平移：物体沿某个方向移动

在三维空间里，旋转用3x3的旋转矩阵R表示，平移用3x1的平移向量t表示。那么一个点P从坐标系A变换到坐标系B，可以写成：

P_B = R * P_A + t

这个公式看着简单，但我得提醒你一句：旋转矩阵R必须是正交矩阵，也就是说R的逆等于它的转置。我在项目中遇到过有人随便填了个矩阵进去，结果点云全飞了，找了半天才发现是旋转矩阵不合法。

注意：旋转矩阵的行列式必须为+1，否则就不是纯旋转，而是包含了镜像或缩放。这在感知系统中会导致严重的定位偏差。

3.2 为什么要引入齐次坐标？

好，问题来了。上面那个公式 P_B = R * P_A + t，你发现没有，它不是一个线性变换。因为有个加法在后面拖着。这就麻烦了——多个变换叠加的时候，公式会变得又臭又长。

举个例子，从坐标系A到B，再到C：

P_C = R2 * (R1 * P_A + t1) + t2
     = R2 * R1 * P_A + R2 * t1 + t2

你看，这才两次变换，就已经有点乱了。要是来个十次八次的，代码里全是括号和加法，调试起来简直要命。

所以，聪明的前辈们引入了齐次坐标。做法很简单：给三维点加一个维度，变成四维：

P = [x, y, z, 1]^T

这样一来，旋转和平移就可以合并到一个4x4的矩阵里：

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

你看，原来需要两步操作（旋转+平移），现在一步矩阵乘法就搞定了。而且多个变换可以连乘：

T_AC = T_BC * T_AB

简洁、优雅、高效。我个人习惯在代码里全部用齐次坐标，省心。

小技巧：在Eigen库中，使用Isometry3d或Affine3d来存储变换矩阵，不要自己手写4x4数组。我见过太多因为数组索引搞错导致的bug了。

3.3 4x4变换矩阵的构建与分解

接下来咱们看看实际怎么用。构建一个4x4变换矩阵，通常有两种方式：

3.3.1 从旋转矩阵和平移向量构建

// C++ Eigen示例
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = rotation_matrix;  // 左上角3x3放旋转
T.block<3,1>(0,3) = translation;      // 右上角3x1放平移
// 最后一行保持 [0, 0, 0, 1]

3.3.2 从轴角和平移构建

// 使用AngleAxis
Eigen::AngleAxisd angle_axis(0.5, Eigen::Vector3d::UnitZ());
Eigen::Matrix3d R = angle_axis.toRotationMatrix();
Eigen::Vector3d t(1.0, 2.0, 3.0);

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(R);
T.pretranslate(t);

反过来，分解变换矩阵也很常用。比如你拿到一个4x4矩阵，想知道旋转和平移分别是多少：

// 分解
Eigen::Matrix3d R = T.block<3,3>(0,0);  // 提取旋转
Eigen::Vector3d t = T.block<3,1>(0,3);  // 提取平移

// 检查旋转矩阵是否合法
double det = R.determinant();
if (std::abs(det - 1.0) > 1e-6) {
    // 有问题！不是纯旋转
}

  核心要点：变换矩阵的逆矩阵表示反向变换。如果你有T_AB（从A到B），那么T_BA = T_AB.inverse()。这在多传感器融合中经常用到，比如把激光雷达点云转到相机坐标系。

3.4 知识体系总览

下面这张图总结了刚体变换和齐次坐标的核心逻辑，我建议你多看几遍：

3.5 避坑指南

最后，分享几个我踩过的坑：

坐标系方向搞反：我曾经把左乘和右乘搞混了。记住，T_AB * P_B = P_A，意思是把B系的点变换到A系。别弄反了。
忘记归一化：齐次坐标最后一位必须是1。如果你做了一些运算导致最后一位不是1了，记得归一化：P = P / P[3]。
精度问题：多次变换连乘后，矩阵可能会因为浮点误差不再满足正交性。我建议每隔一段时间做一次正交化处理。

曾经踩过的坑：有一次我在ROS里发布tf变换，手抖把平移向量写到了旋转矩阵的位置。结果整个点云地图都歪了，排查了整整一个下午。从那以后，我每次构建变换矩阵都会打印出来肉眼检查一遍。

好了，刚体变换和齐次坐标就讲到这里。记住一句话：能用齐次坐标就别用分离的R和t，能省去很多麻烦。

可可精品资料库

📚 课程目录

第三节：刚体变换与齐次坐标

3.1 刚体变换：平移+旋转

3.2 为什么要引入齐次坐标？

3.3 4x4变换矩阵的构建与分解

3.3.1 从旋转矩阵和平移向量构建

3.3.2 从轴角和平移构建

3.4 知识体系总览

3.5 避坑指南