3、坐标变换数学原理：二维旋转矩阵、平移向量、缩放因子、齐次坐标与仿射变换
好，咱们直接进入正题。坐标变换这东西，说白了就是解决一个核心问题：同一个点，在不同坐标系下，坐标值分别是多少？
我在做定位漂移纠正时，最常打交道的就是二维平面上的变换。你想想看，GPS给出的经纬度是一个坐标系，地图上的像素坐标是另一个坐标系，车身上的传感器又有一套坐标系。这三者之间怎么对齐？靠的就是我们今天要讲的这套数学工具。
3.1 二维旋转矩阵
先聊旋转。假设有一个点P，它在原始坐标系下的坐标是(x, y)。现在我把坐标系绕原点逆时针旋转θ角，那么P在新坐标系下的坐标(x', y')是多少？
公式长这样：
x' = x * cosθ - y * sinθ
y' = x * sinθ + y * cosθ写成矩阵形式更清爽：
[x'] [cosθ -sinθ] [x]
[y'] = [sinθ cosθ] [y]这个2x2的矩阵，就是二维旋转矩阵R。
关键点：旋转矩阵是正交矩阵，它的逆矩阵等于它的转置。这意味着R⁻¹ = Rᵀ。这个性质在实际编程中非常有用——你不需要真的去算逆矩阵，转置一下就行，省计算量。
我个人习惯把旋转矩阵记成「上三角是cos和-sin，下三角是sin和cos」。嗯，这个口诀帮我避免了很多次手写公式时的符号错误。
3.2 平移向量
旋转搞定了，那平移呢？平移更简单。如果坐标系的原点移动了，新原点在旧坐标系下的位置是(tx, ty)，那么：
x' = x + tx
y' = y + ty写成向量形式：
P' = P + T其中T = [tx, ty]ᵀ就是平移向量。
避坑指南：我曾经在项目中犯过一个低级错误——把旋转和平移的顺序搞反了。记住：先旋转，再平移。因为旋转是绕原点转的，如果你先平移再旋转，旋转中心就变了，结果完全不对。
3.3 缩放因子
有时候两个坐标系的尺度不一样。比如GPS坐标是经纬度，地图坐标是米，这中间就涉及缩放。缩放因子s表示新坐标系下的单位长度相当于旧坐标系下的s倍：
x' = s * x
y' = s * y写成矩阵：
[x'] [s 0] [x]
[y'] = [0 s] [y]当然，x轴和y轴的缩放可以不同，那就变成：
[x'] [sx 0] [x]
[y'] = [0 sy] [y]这个在纠正GPS漂移时很常见——经度和纬度的缩放因子往往不一样，因为地球不是正球体。
3.4 齐次坐标
现在问题来了。旋转、平移、缩放，这三个操作能不能统一成一个矩阵？
旋转和缩放都是2x2矩阵乘法，但平移是加法。你没法用一个2x2矩阵同时表示乘法和加法。怎么办？
答案就是齐次坐标。
我们把二维点(x, y)扩展成三维向量(x, y, 1)。多出来的这个1，就是齐次坐标的「齐次分量」。这样一来，平移也能写成矩阵乘法了：
[x'] [1 0 tx] [x]
[y'] = [0 1 ty] [y]
[1 ] [0 0 1] [1]旋转和缩放同理：
旋转：[cosθ -sinθ 0] 缩放：[sx 0 0]
[sinθ cosθ 0] [0 sy 0]
[0 0 1] [0 0 1]为什么叫「齐次」？ 因为(x, y, 1)和(2x, 2y, 2)在齐次坐标下表示同一个点。你想想看，把齐次坐标的三个分量都除以最后一个分量，就回到了笛卡尔坐标。这个性质在计算机图形学和定位算法中非常有用，比如处理无穷远点。
3.5 仿射变换
把旋转、平移、缩放组合起来，就得到了仿射变换。它的通用形式是：
[x'] [a b tx] [x]
[y'] = [c d ty] [y]
[1 ] [0 0 1] [1]其中左上角的2x2子矩阵[a b; c d]负责旋转和缩放，右上角的[tx, ty]ᵀ负责平移。最后一行固定为[0, 0, 1]。
仿射变换有几个重要性质：
- 直线还是直线：变换后直线不会变成曲线
- 平行线还是平行线：变换后平行关系保持不变
- 比例不变：线段上的比例关系在变换前后一致
我在做定位漂移纠正时，通常会用仿射变换来建模两个坐标系之间的映射关系。为什么不用更复杂的透视变换？因为对于小范围的定位场景（比如一个停车场、一个园区），仿射变换的精度已经足够了，而且计算量小很多。
注意：仿射变换不能处理「剪切」之外的扭曲。如果你的两个坐标系之间存在非线性畸变（比如镜头畸变），那就需要更复杂的模型了。我曾经在某个项目中试图用仿射变换纠正鱼眼相机的畸变，结果惨不忍睹——后来换成了多项式模型才搞定。
3.6 知识体系总览
下面这张图总结了本章的核心逻辑：
说白了，整个坐标变换的核心就三步：旋转 → 缩放 → 平移。用齐次坐标把它们统一成3x3矩阵，就是仿射变换。这个框架能解决90%以上的定位漂移纠正问题。
我的经验：在实际项目中，我通常不会手动推导这些矩阵。我会用OpenCV的getAffineTransform()函数，或者Eigen库的几何模块。但理解背后的数学原理非常重要——当你的定位结果出现异常时，你能快速判断是旋转算错了，还是平移方向搞反了。
嗯，这一章的内容就到这里。记住：数学是工具，不是目的。理解这些变换的几何意义，比死记硬背公式重要得多。
可可精品资料库