三维空间坐标转换早期笔记

在工业测量和测绘数据处理中，常常涉及三维空间直角坐标系转换问题。本文是上学时刚接触三维测量时的实现方法，现在估计都不会有人这么干了，有现成的优化库和包，调用一下就搞定了。这里算法实现的主要思路就是：最小二乘法+泰勒展开+牛顿法迭代，这里直接用了旋转矩阵中9个参数求解的，实际就是个带二次约束的多元非线性方程组的求解问题。当然，如果不用旋转矩阵中的9个参数和约束条件，直接用3个欧拉角不断迭代也能得到相同的结果，就是那个求偏导公式实在太长了。值得注意的是，本文中初值的选取比较粗暴，实际上有很多有效的方法可以取得更好的初值，让模型收敛的更快。

1 空间坐标转换模型

1.1 空间坐标转换模型的构建

对空间任意两坐标系之间实现空间坐标系之间的转换通常通过平移、旋转、缩放来实现的。

假设有两空间直角坐标系 O−XYZO-XYZO−XYZ 和 o−xyzo-xyzo−xyz，对空间任一点 AAA，在 O−XYZO-XYZO−XYZ 中的坐标为 [X,Y,Z][X, Y, Z][X,Y,Z]，在 o−xyzo-xyzo−xyz 中的坐标为 [x,y,z][x, y, z][x,y,z]。以 ooo 为原点建立一个与空间直角坐标系 O−XYZO-XYZO−XYZ 平行的空间直角坐标系 o−x′y′z′o-x'y'z'o−x′y′z′，AAA 点在该坐标系中的坐标为 [x′,y′,z′][x', y', z'][x′,y′,z′]。

1.1.1 绕坐标轴旋转公式的详细推导

（1）绕 x 轴顺时针旋转角度 α\alphaα

当坐标系绕 x 轴旋转时，x 坐标保持不变，只有 y 和 z 坐标发生变化。在 yz 平面内，设点 PPP 的原始坐标为 (y,z)(y, z)(y,z)，旋转 α\alphaα 角后的坐标为 (y′,z′)(y', z')(y′,z′)。

设点 PPP 到原点的距离为 rrr，点 PPP 与 y 轴的夹角为 θ\thetaθ（从 y 轴逆时针量测），则有：
y=rcos⁡θz=rsin⁡θ \begin{aligned} y &= r\cos\theta \\ z &= r\sin\theta \end{aligned} yz=rcosθ=rsinθ

当坐标系绕 x 轴顺时针 旋转 α\alphaα 角后，y 轴向 z 轴负方向转动，点 PPP 相对于新坐标系 y' 轴的夹角变为 θ+α\theta + \alphaθ+α（坐标系转离了点，夹角变大）。因此新坐标为：
y′=rcos⁡(θ+α)=r(cos⁡θcos⁡α−sin⁡θsin⁡α)=ycos⁡α−zsin⁡αz′=rsin⁡(θ+α)=r(sin⁡θcos⁡α+cos⁡θsin⁡α)=zcos⁡α+ysin⁡α \begin{aligned} y' &= r\cos(\theta + \alpha) = r(\cos\theta\cos\alpha - \sin\theta\sin\alpha) = y\cos\alpha - z\sin\alpha \\ z' &= r\sin(\theta + \alpha) = r(\sin\theta\cos\alpha + \cos\theta\sin\alpha) = z\cos\alpha + y\sin\alpha \end{aligned} y′z′=rcos(θ+α)=r(cosθcosα−sinθsinα)=ycosα−zsinα=rsin(θ+α)=r(sinθcosα+cosθsinα)=zcosα+ysinα

因此，绕 x 轴旋转的变换矩阵为：
Rx(α)=[1000cos⁡α−sin⁡α0sin⁡αcos⁡α] R_x(\alpha) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} Rx(α)= 1000cosαsinα0−sinαcosα

变换公式为：