【转载】基于Bursa模型的七参数空间三维坐标转换

转载自 基于Bursa模型的七参数空间三维坐标转换-CSDN博客

目录

• 一、Bursa模型简介
• 二、Bursa模型的推导
  • 2.1 Bursa坐标转换模型
  • 2.3 建立间接平差模型
    • 注意
  • 2.4 非重合点的坐标转换
• 三、实现过程
  • 3.1 坐标的导入
  • 3.2 解算七参数
  • 3.3 重合点精度评定
  • 3.4 非重合点的转换
  • 3.5 非重合点的精度评定
    • 3.5.1 定权
    • 3.5.2 精度评定
• 四、成果代码

一、Bursa模型简介

模型简介百度即可，这里不做介绍，因为不是自己整理的。

二、Bursa模型的推导

2.1 Bursa坐标转换模型

\[\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}_{C} = \begin{bmatrix} 1 &0 &0 &0 &-Z_{D} &Y_{D} &X_{D} \\ 0 &1 &0 &Z_{D} &0 &-X_{D} &Y_{D} \\ 0 &0 &1 &-Y_{D} &X_{D} &0 &Z_{D} \end{bmatrix} \begin{bmatrix} T_{X} \\ T_{Y} \\ T_{Z} \\ \varepsilon _{X} \\ \varepsilon _{Y} \\ \varepsilon _{Z} \\ m \end{bmatrix} - \begin{bmatrix} X\\ Y\\ Z \end{bmatrix}_{D} \quad(1) \]

2.3 建立间接平差模型

设有 \(N\) 个重合点，七个参数看作必要观测数 $t = 7 $，其中总观测数为 \(n = 3N\), 多余观测数 $r = n − t $。由此关系可知，至少需要3个点才能完成结算。根据间接平差模型，列出误差方程为：

\[\begin{bmatrix} V_{X_{1}} \\ V_{Y_{1}} \\ V_{Z_{1}}\\\vdots \\ V_{X_{N}} \\ V_{Y_{N}} \\ V_{Z_{N}} \end{bmatrix}_{C} = \begin{bmatrix} 1 &0 &0 &0 &-Z_{D_{1}} &Y_{D_{1}} &X_{D_{1}} \\ 0 &1 &0 &Z_{D_{1}} &0 &-X_{D_{1}} &Y_{D_{1}} \\ 0 &0 &1 &-Y_{D_{1}} &X_{D_{1}} &0 &Z_{D_{1}} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 1 &0 &0 &0 &-Z_{D_{N}} &Y_{D_{N}} &X_{D_{N}} \\ 0 &1 &0 &Z_{D_{N}} &0 &-X_{D_{N}} &Y_{D_{N}} \\ 0 &0 &1 &-Y_{D_{N}} &X_{D_{N}} &0 &Z_{D_{N}} \end{bmatrix} \begin{bmatrix} T_{X} \\ T_{Y} \\ T_{Z} \\ \varepsilon _{X} \\ \varepsilon _{Y} \\ \varepsilon _{Z} \\ m \end{bmatrix} - \begin{bmatrix} X_{1}\\ Y_{1}\\ Z_{1} \\ X_{N}\\ Y_{N}\\ Z_{N}\end{bmatrix}_{D} \quad(2) \]

将(2)上式简化为新的矩阵形式为：

\[V=B\hat{X}-L \quad(3) \]

然后利用最小二乘法来求解坐标转换参数，这种方法利用了所有的公共点，可以得到较好的结果，但是由于将每个点的坐标精度都视为精度相同的观测值，所以得到是一种近似的结果。因此，各点的坐标视为同精度独立观测值，P为单位矩阵，则可由(3)式得到：

\[\hat{X}=(B^TB)^{-1}(B^TL) \quad(4) \]

精度评定，其单位权中误差为：

\[\sigma_{0} = \sqrt{\frac{V^TPV}{n-t} } \quad(5) \]

注意

需要将旋转参数 $\varepsilon _{X} $ 、$\varepsilon _{Y} $ 和 $\varepsilon _{Z} $ 的单位为弧度，要将其转换到秒；尺度参数m单位换位“ppm”，ppmpart per million 百万分之……。

具体计算公式为：将旋转角乘以206265即可换为“s”，将尺度参数乘以1000000单位即为 “ppm”。

\[1 弧度(rad) = 206264.8062471 秒 \]

七参数	单位
\(T_{X}\)	m
\(T_{Y}\)	m
\(T_{Z}\)	m
$\varepsilon _{X} $	s
$\varepsilon _{Y} $	s
$\varepsilon _{Z} $	s
m	ppm

2.4 非重合点的坐标转换

利用求得的七参数，将数据代入即可解得转换后的坐标。

精度评定：无法像重合点那样可以利用原有的坐标与转换的坐标来计算残差。此时可利用配置法，将重合点的转换值改正数作为已知值，然后对非公共点进行配置，具体的方法为：

①计算重合点转换值得改正数，其重合点的坐标采用已知值。

\[V = 已知值 - 转换值 \quad(6) \]

②采用配置可计算出非公共点转换值的改正数。

\[V^{'} = \frac{ {\sum_{1}^{n}}P_{i}V_{i} }{ {\sum_{1}^{n}}P_{i} } \quad(7) \]

n为选择重合点的个数，根据非重合点与重合点的距离来定权，其权为：

\[P_{i} = \frac{1}{ {S_{i}}^{2} } \quad(8) \]

三、实现过程

# 忽略烦人的红色提示
import warnings
warnings.filterwarnings("ignore")

# 导入Python的数据处理库pandas，相当于python里的excel
import pandas as pd

# 导入python绘图matplotlib
import matplotlib.pyplot as plt

# 使用ipython的魔法方法，将绘制出的图像直接嵌入在notebook单元格中
%matplotlib inline

# 设置绘图大小
plt.style.use({'figure.figsize':(25,20)})
plt.rcParams['font.sans-serif']=['SimHei']  # 用来正常显示中文标签  
plt.rcParams['axes.unicode_minus']=False  # 用来正常显示负号

3.1 坐标的导入

# 读取CGCS2000坐标系下的坐标
cc = pd.read_csv('new.csv')
# 查看坐标
cc

import numpy as np
np.set_printoptions(suppress=True)
C_XYZ = np.empty((0,3))
for index,row in cc.iterrows():
    pd.set_option('precision', 4) 
    C_x = row['X']
    C_y = row['Y']
    C_z = row['Z']
    C_xyz = np.array([[C_x,C_y,C_z]])
    C_XYZ = np.append(C_XYZ,C_xyz,axis=0)
print(C_XYZ)

# 读取地方坐标系下的坐标
dd = pd.read_csv('old.csv')
# 查看信息
print(dd.shape)
dd

import numpy as np
D_XYZ = np.empty((0,3))
for index,row in dd.iterrows():
    pd.set_option('precision', 4) 
    D_x = row['X']
    D_y = row['Y']
    D_z = row['Z']
    D_xyz = np.array([[D_x,D_y,D_z]])
    D_XYZ = np.append(D_XYZ,D_xyz,axis=0)
print(D_XYZ)

3.2 解算七参数

\[\hat{X}=(B^TB)^{-1}(B^TL) \quad(4) \]

L = []
B = np.empty((0,7))
for i in range(D_XYZ.shape[0]):
    # 提取元素
    X_C = C_XYZ[i][0]
    Y_C = C_XYZ[i][1]
    Z_C = C_XYZ[i][2]
    X_D = D_XYZ[i][0]
    Y_D = D_XYZ[i][1]
    Z_D = D_XYZ[i][2]
    # 构建L矩阵
    L.extend((X_C - X_D,Y_C - Y_D,Z_C - Z_D))
    #L = np.append(L,LL,axis=1)
    # 构建B矩阵
    b1 = np.array([1,0,0,0,-Z_D,Y_D,X_D])
    b2 = np.array([0,1,0,Z_D,0,-X_D,Y_D])
    b3 = np.array([0,0,1,-Y_D,X_D,0,Z_D])
    BB = np.row_stack((b1,b2,b3))
    B = np.append(B,BB,axis=0)
B = B
L = np.array([L]).T
# print("L矩阵为：\n",L)
# print("B矩阵为：\n",B)

a = np.linalg.inv(np.dot(B.T,B))
b = np.dot(B.T,L)
# 求取伪七参数
x = np.dot(a,b)
print("解算的七参数为：")
print("X平移参数：{} m".format(x[0][0]))
print("Y平移参数：{} m".format(x[1][0]))
print("Z平移参数：{} m".format(x[2][0]))
print("X旋转参数：{} s".format(x[3][0]* 206265))
print("Y旋转参数：{} s".format(x[4][0]* 206265))
print("Z旋转参数：{} s".format(x[5][0]* 206265))
print("m尺度参数：{} ppm".format(x[6][0]* 1000000))

3.3 重合点精度评定

\[V=B\hat{X}-L \quad(3) \]

\[\sigma_0=\sqrt{\frac{\mathrm{V}^\mathrm{T}\mathrm{P}\mathrm{V}}{\mathrm{n}-\mathrm{t}}} \quad(5) \]

V = np.dot(B,x)-L
print(V)

import math
xx = math.sqrt(np.dot(V.T,V)/(3*D_XYZ.shape[0]-7))
print(xx)

3.4 非重合点的转换

# 读取地方坐标系下的坐标
ddno = pd.read_csv('oldno.csv')
# 查看信息
print(ddno.shape)
ddno

import numpy as np
DN_XYZ = np.empty((0,3))
for index,row in ddno.iterrows():
    pd.set_option('precision', 4) 
    Dno_x = row['X']
    Dno_y = row['Y']
    Dno_z = row['Z']
    DN_xyz = np.array([[Dno_x,Dno_y,Dno_z]])
    DN_XYZ = np.append(DN_XYZ,DN_xyz,axis=0)
print(DN_XYZ.shape)

\[\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}_{C} = \begin{bmatrix} 1 &0 &0 &0 &-Z_{D} &Y_{D} &X_{D} \\ 0 &1 &0 &Z_{D} &0 &-X_{D} &Y_{D} \\ 0 &0 &1 &-Y_{D} &X_{D} &0 &Z_{D} \end{bmatrix} \begin{bmatrix} T_{X} \\ T_{Y} \\ T_{Z} \\ \varepsilon _{X} \\ \varepsilon _{Y} \\ \varepsilon _{Z} \\ m \end{bmatrix} - \begin{bmatrix} X\\ Y\\ Z \end{bmatrix}_{D} \quad(1) \]

for i in range(DN_XYZ.shape[0]):
    # 提取元素
    XN_D = DN_XYZ[i][0]
    YN_D = DN_XYZ[i][1]
    ZN_D = DN_XYZ[i][2] 
    LN = np.row_stack((XN_D,YN_D,ZN_D))
    # 构建L矩阵
    # 构建B矩阵
    bn1 = np.array([1,0,0,0,-ZN_D,YN_D,XN_D])
    bn2 = np.array([0,1,0,ZN_D,0,-XN_D,YN_D])
    bn3 = np.array([0,0,1,-YN_D,XN_D,0,ZN_D])
    BN = np.row_stack((b1,b2,b3))
    NCC = np.dot(BN,x) + LN
    print('第{}个点的坐标为：{},{},{}'.format(i,NCC[0][0],NCC[1][0],NCC[2][0]))

3.5 非重合点的精度评定

3.5.1 定权

\[V^{'} = \frac{ {\sum_{1}^{n}}P_{i}V_{i} }{ {\sum_{1}^{n}}P_{i} } \quad(7) \]

\[P_{i} = \frac{1}{ {S_{i}}^{2} } \quad(8) \]

aa = list(range(0,V.shape[0],3))
bb = list(range(1,V.shape[0],3))
cc = list(range(2,V.shape[0],3))
# 已知点的x残差
v_x = V[aa,:]
# 已知点的y残差
v_y = V[bb,:]
# 已知点的z残差
v_z = V[cc,:]
v_x.shape

V_xx,V_yy,V_zz = [],[],[]
for i in range(DN_XYZ.shape[0]):
    # 提取元素
    XN_D = DN_XYZ[i][0]
    YN_D = DN_XYZ[i][1]
    ZN_D = DN_XYZ[i][2]
    LC = np.row_stack((XN_D,YN_D,ZN_D))
    # 定权
    PP = np.dot(D_XYZ,LC).T
    ccc = []
    for j in range(PP.shape[1]):
        ccc.append(1/PP[0][j])
    # 公式(7)
    VV_S = sum(ccc)
    print(VV_S)
    V_x = np.dot(np.array([ccc]),v_x)[0][0] / VV_S
    V_y = np.dot(np.array([ccc]),v_y)[0][0] / VV_S
    V_z = np.dot(np.array([ccc]),v_z)[0][0] / VV_S
    V_xx.append(V_x)
    V_yy.append(V_y)
    V_yy.append(V_z)
    print("非重合点GPS{}的残差V_x{}  V_y{}  V_z{} ".format(i+1,V_x,V_y,V_z))

3.5.2 精度评定

# X的中误差
S = 0
for i in V_xx:
    S += i*i
VXX = math.sqrt(S/(len(V_xx)-1))
print("空间直角坐标X残差中误差",VXX)
# Y的中误差
S = 0
for i in V_yy:
    S += i*i
VYY = math.sqrt(S/(len(V_yy)-1))
print("空间直角坐标Y残差中误差",VYY)
# Z的中误差
S = 0
for i in V_zz:
    S += i*i
VZZ = math.sqrt(S/(len(V_zz)-1))
print("空间直角坐标Z残差中误差",VZZ)
# 点位中误差
print("空间直角坐标点位中误差",math.sqrt(VXX**2 + VYY**2 + VZZ**2))

四、成果代码

import pandas as pd
import numpy as np
import math

# C : target
# D : origin
# https://blog.csdn.net/qq_44285092/article/details/105214412

def GetArrayFromCSV(csvPath, title):
    data = pd.read_csv(csvPath)
    XYZArr = np.empty((0, 3))
    for index, row in data.iterrows():
        x = row[title[0]]
        y = row[title[1]]
        z = row[title[2]]
        xyz = np.array([[x, y, z]])
        XYZArr = np.append(XYZArr, xyz, axis=0)
    return XYZArr

# 解算七参数
def CalcBursa7Paras(origin, target):
    L = []
    B = np.empty((0,7))
    for i in  range(origin.shape[0]):
        target_x = target[i][0]
        target_y = target[i][1]
        target_z = target[i][2]
        origin_x = origin[i][0]
        origin_y = origin[i][1]
        origin_z = origin[i][2]
        # 构建L矩阵
        L.extend((target_x - origin_x, target_y - origin_y, target_z - origin_z))
        b1 = np.array([1,0,0,   0,          -origin_z,  origin_y,   origin_x])
        b2 = np.array([0,1,0,   origin_z,   0,          -origin_x,  origin_y])
        b3 = np.array([0,0,1,   -origin_y,  origin_x,   0,          origin_z])
        BB = np.row_stack((b1,b2,b3))
        B = np.append(B,BB,axis=0)
    L = np.array([L]).T

    a = np.linalg.inv(np.dot(B.T, B))
    b = np.dot(B.T, L)
    result = np.dot(a, b)
    V = np.dot(B,result)-L

    err = math.sqrt(np.dot(V.T,V)/(3*origin.shape[0]-7))
    print("解算的七参数为: ")
    print("X平移参数: {} m".format(result[0][0]))
    print("Y平移参数: {} m".format(result[1][0]))
    print("Z平移参数: {} m".format(result[2][0]))
    print("X旋转参数: {} s".format(result[3][0]* 206265))
    print("Y旋转参数: {} s".format(result[4][0]* 206265))
    print("Z旋转参数: {} s".format(result[5][0]* 206265))
    print("m尺度参数: {} ppm".format(result[6][0]* 1000000))
    print("中误差: {}".format(err))
    return result, err, V


# 计算坐标
def CalcBursaTargin(origin, param7, p = True):

    for i in range(origin.shape[0]):
        x = origin[i][0]
        y = origin[i][1]
        z = origin[i][2]
        LN = np.row_stack((x,y,z))

        # 构建L矩阵
        # 构建B矩阵
        b1 = np.array([1,0,0,   0,  -z, y,  x])
        b2 = np.array([0,1,0,   z,  0,  -x, y])
        b3 = np.array([0,0,1,   -y, x,  0,  z])
        B = np.row_stack((b1, b2, b3))
        NC = np.dot(B, param7) + LN
        if p : print('第{}个点的坐标为: {},{},{}'.format(i,NC[0][0],NC[1][0],NC[2][0]))
    

# 精度评定
def CalcBursaAccuracy(origin, V):
    v_x = V[list(range(0, V.shape[0], 3)),:]
    v_y = V[list(range(1, V.shape[0], 3)),:]
    v_z = V[list(range(2, V.shape[0], 3)),:]
    V_xx,V_yy,V_zz = [],[],[]
    for i in range(origin.shape[0]):
        x = origin[i][0]
        y = origin[i][1]
        z = origin[i][2]
        LC = np.row_stack((x,y,z))
        # 定权
        PP = np.dot(origin, LC).T
        ccc = []
        for j in range(PP.shape[1]):
            ccc.append(1/PP[0][j])
        VV_S = sum(ccc)
        V_x = np.dot(np.array([ccc]), v_x)[0][0] / VV_S
        V_y = np.dot(np.array([ccc]), v_y)[0][0] / VV_S
        V_z = np.dot(np.array([ccc]), v_z)[0][0] / VV_S
        V_xx.append(V_x)
        V_yy.append(V_y)
        V_zz.append(V_z)
        # print("非重合点GPS{}的残差V_x{}  V_y{}  V_z{} ".format(i+1,V_x,V_y,V_z))
    
    S_X, S_Y, S_Z = 0, 0, 0
    for i in V_xx:
        S_X += i * i
    VXX = math.sqrt(S_X/(len(V_xx) -1))
    for i in V_yy:
        S_Y += i * i
    VYY = math.sqrt(S_Y/(len(V_yy) -1))
    for i in V_zz:
        S_Z += i * i
    VZZ = math.sqrt(S_Z/(len(V_zz) -1))

    VXYZ = math.sqrt(VXX**2 + VYY**2 + VZZ**2)
    print("\n配置法中误差：")
    print("空间直角坐标X残差中误差",VXX)
    print("空间直角坐标Y残差中误差",VYY)
    print("空间直角坐标Z残差中误差",VZZ)
    print("空间直角坐标点位中误差",VXYZ)