二维坐标变换：旋转矩阵、平移向量、齐次坐标
各位同学，今天我们来聊聊二维坐标变换。说实话，这是感知融合里最基础、也最容易被忽视的一块。我见过不少同事，算法调得飞起，结果坐标一变换，车的位置直接偏到马路牙子上去了。嗯，咱们今天就把这块地基打牢。
1. 旋转矩阵：让坐标系转起来
先问大家一个问题：一辆车上的激光雷达，和车本身的坐标系，方向一样吗？
答案是不一定。雷达安装时可能有俯仰、偏航。在二维世界里，我们只关心绕Z轴的旋转，也就是偏航角θ。
旋转矩阵长这样：
R = | cosθ -sinθ |
| sinθ cosθ |它的作用很简单：把一个点(x, y)绕原点旋转θ角度。新坐标(x', y') = R * (x, y)。
我个人习惯把旋转矩阵记成「余弦在主角，正弦在对角」。为什么？因为cosθ永远在矩阵的主对角线上，sinθ在对角线上，但符号不同。
核心要点：旋转矩阵是正交矩阵，它的逆等于它的转置。这意味着R⁻¹ = Rᵀ。这个性质在实际工程中非常有用——你不需要真的去求逆矩阵，转置一下就行，计算量小得多。
我在项目中遇到过一个问题：雷达点云和相机图像对不上。排查了半天，发现是旋转矩阵的符号搞反了。顺时针旋转和逆时针旋转，sinθ的符号正好相反。你想想看，一个符号错了，整个融合结果就全歪了。
2. 平移向量：把坐标系挪个位置
旋转解决的是方向问题，平移解决的是位置问题。
假设激光雷达装在车顶前方0.5米处。那么雷达坐标系的原点，相对于车辆坐标系的原点，就有一个偏移量。这个偏移量就是平移向量t = [tx, ty]ᵀ。
变换公式很简单：
x' = x + tx
y' = y + ty说白了，就是把点挪过去。
避坑指南：我曾经在标定的时候，把平移向量的单位搞错了。雷达输出的是毫米，车辆坐标系用的是米。结果一变换，点云全飞到天上了。后来我养成了一个习惯：所有传感器数据进来，第一件事就是统一单位。毫米转米，或者米转毫米，必须明确。
3. 齐次坐标：把旋转和平移合二为一
刚才我们做了两步：先旋转，再平移。写成公式就是：
x' = R * x + t这个形式虽然简洁，但有个问题：它不是线性变换。因为有个加法t在里面。如果我们有一连串的变换（比如从雷达到车辆，再从车辆到世界），就得反复做乘法和加法，很麻烦。
齐次坐标就是来解决这个问题的。
它的思路很简单：给二维点(x, y)加一个维度，变成(x, y, 1)。然后变换矩阵变成3x3的：
T = | R t |
| 0 1 |展开就是：
| x' | | cosθ -sinθ tx | | x |
| y' | = | sinθ cosθ ty | | y |
| 1 | | 0 0 1 | | 1 |这样一来，旋转和平移就合并成了一个矩阵乘法。连续变换只需要把矩阵乘起来就行。
为什么用齐次坐标？因为矩阵乘法可以串联。你想想看，从雷达到车辆是一个矩阵T_rv，从车辆到世界是T_vw。那么从雷达直接到世界就是T_rw = T_vw * T_rv。一次乘法搞定，不用写循环，不用判断边界。代码干净，效率高。
4. 逆变换：从目标坐标系回到源坐标系
有正向变换，就一定有逆向变换。比如我们知道了车辆坐标系下的点，想反推它在雷达坐标系下的位置。
逆变换矩阵就是T的逆矩阵：
T⁻¹ = | Rᵀ -Rᵀ * t |
| 0 1 |注意，这里利用了旋转矩阵的正交性：R⁻¹ = Rᵀ。所以逆变换的计算量很小。
重要提醒：千万不要直接对T求逆！虽然数学上可行，但数值稳定性差。尤其是当平移量很大时，直接求逆会引入数值误差。我建议用公式手动计算逆矩阵，又快又准。
5. 知识体系图
下面这张图展示了二维坐标变换的核心逻辑：
6. 代码示例：C++实现
下面是一个简单的二维变换实现。我在实际项目中就是这么写的：
#include <Eigen/Dense>
#include <cmath>
// 构建旋转矩阵
Eigen::Matrix2d buildRotationMatrix(double theta) {
Eigen::Matrix2d R;
R << cos(theta), -sin(theta),
sin(theta), cos(theta);
return R;
}
// 构建齐次变换矩阵
Eigen::Matrix3d buildTransform(double theta, double tx, double ty) {
Eigen::Matrix3d T = Eigen::Matrix3d::Identity();
T.block<2,2>(0,0) = buildRotationMatrix(theta);
T(0,2) = tx;
T(1,2) = ty;
return T;
}
// 正向变换
Eigen::Vector2d transformPoint(const Eigen::Vector2d& p,
const Eigen::Matrix3d& T) {
Eigen::Vector3d p_homo(p.x(), p.y(), 1.0);
Eigen::Vector3d p_trans = T * p_homo;
return Eigen::Vector2d(p_trans.x(), p_trans.y());
}
// 逆变换（用公式计算，避免直接求逆）
Eigen::Matrix3d inverseTransform(const Eigen::Matrix3d& T) {
Eigen::Matrix2d R = T.block<2,2>(0,0);
Eigen::Vector2d t(T(0,2), T(1,2));
Eigen::Matrix3d T_inv = Eigen::Matrix3d::Identity();
T_inv.block<2,2>(0,0) = R.transpose(); // R⁻¹ = Rᵀ
T_inv.block<2,1>(0,2) = -R.transpose() * t;
return T_inv;
}个人经验：我习惯用Eigen库做矩阵运算。它的block操作非常方便，而且编译优化后性能很好。如果你用ROS，tf2库也封装了这些变换，但底层原理是一样的。理解原理，用起来才放心。
7. 总结
二维坐标变换，说白了就是三件事：旋转、平移、合起来。旋转用矩阵，平移用向量，合起来用齐次坐标。
嗯，这里要注意：齐次坐标的最后一维是1，不是0。如果是0，表示方向向量（只旋转不平移）。这个区别在感知融合中经常用到——比如表示一个朝向时，我们用方向向量，最后一维就是0。
好了，二维变换就讲到这里。下一章我们进入三维世界，那里有更多有意思的东西等着我们。
可可精品资料库