间接平差求解七参数模型

1.前言

如前所述，本文主要介绍七参数求解的间接平差方法。本文的七参数模型如式（1）所示：
$${\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_s=k\ast \left[\begin{array}{ccc}1& 0& 0\\ 0& cosa& -sina\\ 0& sina& cosa\end{array}\right]\left[\begin{array}{ccc}cosb& 0& sinb\\ 0& 1& 0\\ -sinb& 0& cosb\end{array}\right]\left[\begin{array}{ccc}cosc& -sinc& 0\\ sinc& cosc& 0\\ 0& 0& 1\end{array}\right]{\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_o+\left[\begin{array}{c}Tx\\ Ty\\ Tz\end{array}\right]\text{\hspace{1em}(1)}$$

2. 平差函数模型

对式(1)进行简化，另
$$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& cosa& -sina\\ 0& sina& cosa\end{array}\right]\left[\begin{array}{ccc}cosb& 0& sinb\\ 0& 1& 0\\ -sinb& 0& cosb\end{array}\right]\left[\begin{array}{ccc}cosc& -sinc& 0\\ sinc& cosc& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
则式(1)可化为：
$${\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_s=k\ast R{\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_o+\left[\begin{array}{c}Tx\\ Ty\\ Tz\end{array}\right]\text{\hspace{1em}(3)}$$
对其进行一阶泰勒展开，有
$${\left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right]}_s+{\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_s=k^0\ast R^0{\left[\begin{array}{c}{X}^0\\ Y^0\\ Z^0\end{array}\right]}_o+\left[\begin{array}{c}T{x}^0\\ T{y}^0\\ T{z}^0\end{array}\right]+R^0{\left[\begin{array}{c}{X}^0\\ Y^0\\ Z^0\end{array}\right]}_o\ast dk+k^0\ast dR{\left[\begin{array}{c}{X}^0\\ Y^0\\ Z^0\end{array}\right]}_o+\left[\begin{array}{c}{d}_{T}x\\ d_{T}y\\ d_{T}z\end{array}\right]\text{\hspace{1em}(4)}$$

3. 代码形式

将式(4)化为参数方程，则有
$$v=Bx-l\text{\hspace{1em}(5)}$$
的形式。式中，B、x、l的具体形式比较复杂，代码如下所示:

def makeBL(X,S1,S2):           #S1向S2转换。X中的旋转参数为角度，以度为单位
    # a,b,c为三轴的角度，以弧度为单位；Tx,Ty,Tz为平移量,k为缩放系数
    Tx,Ty,Tz,a,b,c,k=X[0,0],X[1,0],X[2,0],X[3,0]*np.pi/180,X[4,0]*np.pi/180,X[5,0]*np.pi/180,X[6,0]
    # Tx = np.mean(S2,0)[0]-np.mean(S1,0)[0]
    # Ty = np.mean(S2,0)[1]-np.mean(S1,0)[1]
    # Tz = np.mean(S2,0)[2]-np.mean(S1,0)[2]
    m=np.size(S1,0)# 判断公共点的个数
    B=np.zeros([3*m,7])
    L=np.zeros([3*m,1])
    for i in range(m):
        B[3*i,0],B[3*i+1,1],B[3*i+2,2] = 1,1,1
        # M部分
        B[3*i,3]=0
        B[3*i,4]=(-np.sin(b)*np.cos(c))*S1[i,0]+(np.sin(b)*np.sin(c))*S1[i,1]+(np.cos(b))*S1[i,2]
        B[3*i,5]=(-np.cos(b)*np.sin(c))*S1[i,0]-(np.cos(b)*np.cos(c))*S1[i,1]
        B[3*i+1,3]=(-np.sin(a)*np.sin(c)+np.cos(a)*np.sin(b)*np.cos(c))*S1[i,0]+(-np.sin(a)*np.cos(c)-np.cos(a)*np.sin(b)*np.sin(c))*S1[i,1]+(-np.cos(a)*np.cos(b))*S1[i,2]
        B[3*i+1,4]=(np.sin(a)*np.cos(b)*np.cos(c))*S1[i,0]+(-np.sin(a)*np.cos(b)*np.sin(c))*S1[i,1]+(np.sin(a)*np.sin(b))*S1[i,2]
        B[3*i+1,5]=(np.cos(a)*np.cos(c)-np.sin(a)*np.sin(b)*np.sin(c))*S1[i,0]+(-np.cos(a)*np.sin(c)-np.sin(a)*np.sin(b)*np.cos(c))*S1[i,1]
        B[3*i+2,3]=(np.cos(a)*np.sin(c)+np.sin(a)*np.sin(b)*np.cos(c))*S1[i,0]+(np.cos(a)*np.cos(c)-np.sin(a)*np.sin(b)*np.sin(c))*S1[i,1]+(-np.sin(a)*np.cos(b))*S1[i,2]
        B[3*i+2,4]=(-np.cos(a)*np.cos(b)*np.cos(c))*S1[i,0]+(np.cos(a)*np.cos(b)*np.sin(c))*S1[i,1]+(-np.cos(a)*np.sin(b))*S1[i,2]
        B[3*i+2,5]=(np.sin(a)*np.cos(c)+np.cos(a)*np.sin(b)*np.sin(c))*S1[i,0]+(-np.sin(a)*np.sin(c)+np.cos(a)*np.sin(b)*np.cos(c))*S1[i,1]
        B[3*i:3*i+3,3:6]=B[3*i:3*i+3,3:6]*k
        #N部分
        D=RR(X[3,0],X[4,0],X[5,0])
        B[3*i,6]=D[0,0]*S1[i,0]+D[0,1]*S1[i,1]+D[0,2]*S1[i,2]
        B[3*i+1,6]=D[1,0]*S1[i,0]+D[1,1]*S1[i,1]+D[1,2]*S1[i,2]
        B[3*i+2,6]=D[2,0]*S1[i,0]+D[2,1]*S1[i,1]+D[2,2]*S1[i,2]
        #L部分
        L[3*i,0]=-(Tx+k*B[3*i,6]-S2[i,0])
        L[3*i+1,0]=-(Ty+k*B[3*i+1,6]-S2[i,1])
        L[3*i+2,0]=-(Tz+k*B[3*i+2,6]-S2[i,2])
        
        B=np.matrix(B)
        L=np.matrix(L)
    return B,L

 # a,b,c为三轴的角度，以弧度为单位；Tx,Ty,Tz为平移量,k为缩放系数
    Tx,Ty,Tz,a,b,c,k=X[0,0],X[1,0],X[2,0],X[3,0]*np.pi/180,X[4,0]*np.pi/180,X[5,0]*np.pi/180,X[6,0]

4. 完整代码

基于Python，已经将这个算法写成了函数，只要输入公点的两套坐标，即可实现坐标转换参数的求解。具体的代码为

def RR(A,B,C):  # A,B,C为角度，旋转矩阵R的定义
        a,b,c=A*np.pi/180, B*np.pi/180, C*np.pi/180
        RR=np.zeros([3,3])  
        RR[0,0]=np.cos(b)*np.cos(c)
        RR[0,1]=-np.cos(b)*np.sin(c)
        RR[0,2]=np.sin(b)   
        RR[1,0]=np.cos(a)*np.sin(c)+np.sin(a)*np.sin(b)*np.cos(c)
        RR[1,1]=np.cos(a)*np.cos(c)-np.sin(a)*np.sin(b)*np.sin(c)
        RR[1,2]=-np.sin(a)*np.cos(b)   
        RR[2,0]=np.sin(a)*np.sin(c)-np.cos(a)*np.sin(b)*np.cos(c)
        RR[2,1]=np.sin(a)*np.cos(c)+np.cos(a)*np.sin(b)*np.sin(c)
        RR[2,2]=np.cos(a)*np.cos(b)
        return RR

def makeBL(X,S1,S2):           #S1向S2转换。X中的旋转参数为角度，以度为单位
    # a,b,c为三轴的角度，以弧度为单位；Tx,Ty,Tz为平移量,k为缩放系数
    Tx,Ty,Tz,a,b,c,k=X[0,0],X[1,0],X[2,0],X[3,0]*np.pi/180,X[4,0]*np.pi/180,X[5,0]*np.pi/180,X[6,0]
    
    m=np.size(S1,0)# 判断公共点的个数
    B=np.zeros([3*m,7])
    L=np.zeros([3*m,1])
    for i in range(m):
        B[3*i,0],B[3*i+1,1],B[3*i+2,2] = 1,1,1
        # M部分
        B[3*i,3]=0
        B[3*i,4]=(-np.sin(b)*np.cos(c))*S1[i,0]+(np.sin(b)*np.sin(c))*S1[i,1]+(np.cos(b))*S1[i,2]
        B[3*i,5]=(-np.cos(b)*np.sin(c))*S1[i,0]-(np.cos(b)*np.cos(c))*S1[i,1]
        B[3*i+1,3]=(-np.sin(a)*np.sin(c)+np.cos(a)*np.sin(b)*np.cos(c))*S1[i,0]+(-np.sin(a)*np.cos(c)-np.cos(a)*np.sin(b)*np.sin(c))*S1[i,1]+(-np.cos(a)*np.cos(b))*S1[i,2]
        B[3*i+1,4]=(np.sin(a)*np.cos(b)*np.cos(c))*S1[i,0]+(-np.sin(a)*np.cos(b)*np.sin(c))*S1[i,1]+(np.sin(a)*np.sin(b))*S1[i,2]
        B[3*i+1,5]=(np.cos(a)*np.cos(c)-np.sin(a)*np.sin(b)*np.sin(c))*S1[i,0]+(-np.cos(a)*np.sin(c)-np.sin(a)*np.sin(b)*np.cos(c))*S1[i,1]
        B[3*i+2,3]=(np.cos(a)*np.sin(c)+np.sin(a)*np.sin(b)*np.cos(c))*S1[i,0]+(np.cos(a)*np.cos(c)-np.sin(a)*np.sin(b)*np.sin(c))*S1[i,1]+(-np.sin(a)*np.cos(b))*S1[i,2]
        B[3*i+2,4]=(-np.cos(a)*np.cos(b)*np.cos(c))*S1[i,0]+(np.cos(a)*np.cos(b)*np.sin(c))*S1[i,1]+(-np.cos(a)*np.sin(b))*S1[i,2]
        B[3*i+2,5]=(np.sin(a)*np.cos(c)+np.cos(a)*np.sin(b)*np.sin(c))*S1[i,0]+(-np.sin(a)*np.sin(c)+np.cos(a)*np.sin(b)*np.cos(c))*S1[i,1]
        B[3*i:3*i+3,3:6]=B[3*i:3*i+3,3:6]*k
        #N部分
        D=RR(X[3,0],X[4,0],X[5,0])
        B[3*i,6]=D[0,0]*S1[i,0]+D[0,1]*S1[i,1]+D[0,2]*S1[i,2]
        B[3*i+1,6]=D[1,0]*S1[i,0]+D[1,1]*S1[i,1]+D[1,2]*S1[i,2]
        B[3*i+2,6]=D[2,0]*S1[i,0]+D[2,1]*S1[i,1]+D[2,2]*S1[i,2]
        #L部分
        L[3*i,0]=-(Tx+k*B[3*i,6]-S2[i,0])
        L[3*i+1,0]=-(Ty+k*B[3*i+1,6]-S2[i,1])
        L[3*i+2,0]=-(Tz+k*B[3*i+2,6]-S2[i,2])
        
        B=np.matrix(B)
        L=np.matrix(L)
    return B,L

def Trans(S1,S2): #S1先S2转换，其中S1、S2为mX3的二维数组。返回7参数。
    X=np.array([[0,0,0,0,0,0,1]]).T #7参数的初值
    dX = X # dX为转换参数X的改正数
    j=1
    while min(dX[0:3,0])>0.05 or min(dX[3:6,0])*3600>10**(-7) or dX[6,0]>10**(-7):
        B,L=makeBL(X,S1,S2)
        N,W = B.T*B,B.T*L
        dX=(np.linalg.pinv(N)*W).A   #转换参数的改正数，旋转参数的角度
        X=X+dX
        dX=abs(dX)
        j=j+1
        if j==2000:
            break

5. 方法讨论

这个代码的主要问题有以下两个：
（1）缺少观测值的权阵，这个可以解决。
(2) 进过测试，对于多数的论文案列，改法求解转换参数的精度为0.5~5mm。对于某些模型数据，改法转换精度的误差比较大。因此适用性有限。