【7参数转换】三维空间坐标相似变换

一、最小二乘法旋转参数确定

1.模型-空间相似变换

$$\begin{gathered} X=X_0+m\cdot R\cdot x \\ \to \left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]+m\cdot \left[\begin{array}{c}{r}_{11}{r}_{12}{r}_{13}\\ r_{21}{r}_{22}{r}_{23}\\ r_{31}{r}_{32}{r}_{33}\end{array}\right]\cdot \left[\begin{array}{c}x\\ y\\ z\end{array}\right] \end{gathered}$$

其中$R$为旋转矩阵，$m$为尺度参数，$[X_0,Y_0,Z_0{]}^{\prime }$是平移参数，一共有7个。

2.旋转矩阵$R$

旋转矩阵的确定有两种方式，一种是利用旋转角进行确定，另一种是通过4参数附加一个约束进行确定。

2.1 旋转角确定Rotation matrix using trigonometric functions

$$\begin{gathered} R=R_{\omega }\cdot R_{\phi }\cdot R_{\kappa } \\ =\left[\begin{array}{c}cos\kappa -sin\kappa 0\\ sin\kappa -cos\kappa 0\\ 001\end{array}\right]\cdot \left[\begin{array}{c}cos\phi 0-sin\phi \\ 010\\ -sin\phi 0cos\phi \end{array}\right]\cdot \left[\begin{array}{c}100\\ 0cos\omega -sin\omega \\ 0sin\omega cos\omega \end{array}\right] \\ =\left[\begin{array}{c}{r}_{11}{r}_{12}{r}_{13}\\ r_{21}{r}_{22}{r}_{23}\\ r_{31}{r}_{32}{r}_{33}\end{array}\right] \\ =\left[\begin{array}{c}cos\phi cos\kappa -cos\phi sin\kappa sin\phi \\ cos\omega sin\kappa +sin\omega sin\phi cos\kappa cos\omega cos\kappa -sin\omega sin\phi sin\kappa -sin\omega \cos \phi \\ sin\omega sin\kappa -cos\omega \sin \phi \cos \kappa sin\omega \cos \kappa +\cos \omega \sin \phi \sin \kappa cos\omega \cos \phi \end{array}\right] \end{gathered}$$

其中$\kappa$是绕Z轴逆时针旋转的角度，$\phi$是绕Y轴逆时针旋转的角度，中$\omega$是绕X轴逆时针旋转的角度。

2.2代数参数确定Rotation matrix using algebraic functions

四参数的方法：
$$R=\left[\begin{array}{c}{d}^2+a^2-b^2-c^{2}2(ab-cd)2(ac+bd)\\ 2(ab+cd)d^2-a^2+b^2-c^{2}2(bc-ad)\\ 2(ac-bd)2(bc+ad)d^2-a^2-b^2+c^2\end{array}\right]$$
其中有一个约束条件$m=a^2+b^2+c^2+d^2=1$

在四参数与几何旋转角$\kappa 、\phi 、\omega$之间的转换可以根据旋转矩阵$R$唯一，从而一一对应出结果：
$$\begin{gathered} cos\phi \cdot sin\kappa =2(dc-ab) \\ cos\phi \cdot cos\kappa =d^2+a^2-b^2-c^2 \\ cos\phi \cdot sin\omega =2(da-bc) \\ cos\phi \cdot cos\omega =d^2-a^2-b^2+c^2 \\ sin\phi =2(ac+bd) \end{gathered}$$
四参数转换的优势在于无需使用三角函数、简化了计算过程和并且 具有较快的收敛，无奇异点。In summary, a rotation matrix with algebraic functions offers the following benefits: 1. no use of trigonometric functions, 2. simplified computation of the design matrix and faster convergence in adjustment systems, 3.no singularities, 4.faster computation by avoiding power series for internal trigonometric calculations.

3.平差模型

在进行某一坐标系点位$(x,y,z)$到另一坐标系点位$(X,Y,Z)$的转换过程中，由于只需要7个参数，所以对应点位数目只需要 3个就可实现。本次编程实现，采用四参数模型作为旋转矩阵的确定方式，参与最小二乘确定最终的参数，因为四个参数之间存在约束，所以平差模型选用附有约束条件的间接平差方法。原理为：
$$\begin{gathered} 观测量误差方程：A\cdot \hat{x}-l=v \\ 约束条件方程：B\cdot \hat{x}+w=0 \\ Gauss-Markov模型的条件：v^T\cdot P\cdot v+2k\cdot (B\cdot \hat{x}+w)\to min \\ 结果：\left[\begin{array}{c}{A}^T\cdot P\cdot A{B}^T\\ B0\end{array}\right]\cdot \left[\begin{array}{c}\hat{x}\\ k\end{array}\right]+\left[\begin{array}{c}-A^T\cdot P\cdot l\\ w\end{array}\right]=0 \end{gathered}$$
所以$\hat{x}$的计算只需要对系数矩阵求逆乘上后一个矩阵进而取前8个参数（$X_0,Y_0,Z_0,m,a,b,c,d$）即可。

其中
$$A_i=\left[\begin{array}{c}100y(2ab-2cd)+z(2ac+2bd)+x(a^2-b^2-c^2+d^2)m(2ax+2by+2cz)m(2ay-2bx+2dz)-m(2cx-2az+2dy)m(2bz-2cy+2dx)\\ 010x(2ac-2bd)-z(2ad-2bc)-y(a^2-b^2+c^2-d^2)-m(2ay-2cx+2dz)m(2by-2dx+2cz)m(2ax+2bz-2cy)-m(2bx+2az-2dy)\\ 001x(2ac-2bd)+y(2ad+2bc)-z(a^2+b^2-c^2-d^2)m(2cx-2az+2dy)-m(2bz-2cy+2dx)m(2ax+2by+2cz)m(2ay-2bx+2dz)\end{array}\right]$$

$$B=\left[00002a2b2c2d\right]$$

$w={\tilde{a}}^2+{\tilde{b}}^2+{\tilde{c}}^2+{\tilde{d}}^2-1$

$l=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]-\tilde{m}\cdot \tilde{R}\cdot \left[\begin{array}{c}x\\ y\\ z\end{array}\right]-\left[\begin{array}{c}{\tilde{X}}_0\\ {\tilde{Y}}_0\\ {\tilde{Z}}_0\end{array}\right]$

其中参数上方的波浪线表示参数的近似值。通过不断迭代，当参数的改正数小于某个阈值时停止迭代即可。

4.参数近似值的计算

由于存在平差模型是否收敛的问题，所以参数的近似值不能够随意选取，需要尽可能接近真实值，所以需要对参数的初始近似值进行获取。此处获取需要才用到中间坐标系，但总的来说，可以通过3个对应点组通过计算获取。
旋转矩阵的获取需要用到计算正交矩阵：

通过以上方法，可以获取到旋转矩阵的近似值和其他四个参数的近似值，于是可以计算出$l$矩阵，收敛性质较好。

5.算例

6.其他基础知识

6.1叉乘公式

6.2由旋转矩阵$R$推四元数

对应$d$，$x$对应$a$，$y$对应$b$，$z$对应$c$。

7.程序实现。

【7参数转换】三维空间坐标相似变换
程序结果呈现。