Foreword: In the previous coordinate system conversion, the use of the seven-parameter model is only effective for the coordinate conversion of small rotation angles. Therefore, it is necessary to improve the seven-parameter model to adapt to large rotation angles, emm. I have borrowed ideas from the papers of two big guys from Wuhan University (Wuhan University is indeed the number one college in surveying and mapping. There are so many talents), and I will quote it here.
[1] Liu Lei, He Zhanguo, Zheng Zuoya, Jia Chuanliang. An iterative solution for large-angle three-dimensional coordinate transformation parameters [J]. Surveying and Mapping Science, 2021, 46(06): 65-69+76. DOI: 10.16251/j.cnki.1009-2307.2021.06.010.
[2] Yao Yibin, Huang Chengmeng, Li Chengchun, Kong Jian. A 3D Coordinate Transformation Parameter Solving Algorithm for Large Angles [J]. Journal of Wuhan University (Information Science Edition), 2012, 37(03): 253-256. DOI: 10.13203/j.whugis2012.03.008.
Here we mainly implement the algorithm in the first paper.
1. The idea of realizing a large rotation angle
Since the Bursa seven-parameter model is only applicable to small rotations, it is only necessary to modify the rotation matrix to apply large rotation angles. In the original article, the Rodri matrix R composed of antisymmetric matrices is proposed to replace the seven-parameter rotation matrix, and it is proved that the matrix has the characteristics of a rotation matrix. You can see the original article for the specific formula.
2. Program implementation
code show as below.
import numpy as np
# 定义一个函数Large Angle coordinate conversion
def LACC(oldXYZ, newXYZ):
m = len(oldXYZ)
if m < 3:
print('少于三个点')
return
if len(oldXYZ) != len(newXYZ):
print('控制点坐标数量不同')
return
# 初值
k = 1
T = np.array([0, 0, 0]).reshape((3, 1))
a = b = c = 0
# 提取原、新坐标系坐标，方面调用
XA = np.array(oldXYZ)[:, 0]
YA = np.array(oldXYZ)[:, 1]
ZA = np.array(oldXYZ)[:, 2]
XB = np.array(newXYZ)[:, 0]
YB = np.array(newXYZ)[:, 1]
ZB = np.array(newXYZ)[:, 2]
# 权矩阵
P = np.eye(3 * m)
# 迭代参数差值的初值
dTx = dTy = dTz = 2
da = db = dc = 0.0001
dk = 0.0001
# 迭代，这里选择平移参数迭代差值小于1，旋转abc小于1e-5（作者原文为1e-8），k小于1e-5（原文1e-7）
while dTx > 1 or dTy > 1 or dTz > 1 or da > 0.00001 or db > 0.00001 or dc > 0.00001 or dk > 0.00001:
# 构造S，R矩阵
S = np.array([0, -float(c), -float(b), float(c), 0, -float(a), float(b), float(a), 0]).reshape((3, 3))
E = np.eye(3)
R = npmulti((E + S), np.linalg.inv(E - S))
# 构造B矩阵(3mx7)
# L矩阵（3mx1)
SE = npmulti(S, -1) + E
B1 = np.zeros((3 * m, 3))
B1[0:m, :] = SE[0, :]
B1[m:2 * m, :] = SE[1, :]
B1[2 * m:3 * m, :] = SE[2, :]
B2 = np.zeros((3 * m, 4))
L = np.zeros((3 * m, 1))
for i in range(0, m):
B2[i][1] = -k * ZA[i] - ZB[i] + T[2]
B2[i][2] = -k * YA[i] - YB[i] + T[1]
B2[i][3] = XA[i] - c * YA[i] - b * ZA[i]
B2[m + i][0] = -k * ZA[i] - ZB[i] + T[2]
B2[m + i][2] = k * XA[i] + XB[i] - T[0]
B2[m + i][3] = c * XA[i] + YA[i] - a * ZA[i]
B2[2 * m + i][0] = k * YA[i] + YB[i] - T[1]
B2[2 * m + i][1] = k * XA[i] + XB[i] - T[0]
B2[2 * m + i][3] = b * XA[i] + a * YA[i] + ZA[i]
L[i] = -(k * XA[i] - k * c * YA[i] - k * b * ZA[i] - XB[i] + T[0] - c * YB[i] + c * T[1] - b * ZB[i] + b *
T[2])
L[m + i] = -(
k * c * XA[i] + k * YA[i] - k * a * ZA[i] + c * XB[i] - c * T[0] - YB[i] + T[1] - a * ZB[i] + a * T[
2])
L[2 * m + i] = -(
k * b * XA[i] + k * a * YA[i] + k * ZA[i] + b * XB[i] - b * T[0] + a * YB[i] - a * T[1] - ZB[i] + T[
2])
B = np.c_[B1, B2]
# 求解法方程x=(Bnn-1)*W
Nbb = npmulti(B.T, P, B)
W = npmulti(B.T, P, L)
x = npmulti(np.linalg.inv(Nbb), W)
dTx, dTy, dTz, da, db, dc, dk = x[0:7]
T = T + np.array([float(dTx), float(dTy), float(dTz)]).reshape((3, 1))
a += da
b += db
c += dc
k += dk
return T, R, float(k)
# 自己写的一个函数，可以实现任意个矩阵相乘，前提条件第一个参数a为矩阵（写判断很麻烦，不如制定规则）
def npmulti(a, *par):
# a为矩阵
multi = a
for p in par:
if isinstance(p, np.ndarray):
multi = np.matmul(multi, p)
elif isinstance(p, int) or isinstance(p, float):
multi = np.dot(multi, p)
return multi
3. Calculation results
Here select 5 pairs of points in the original text to test, the code is as follows.
if __name__ == '__main__':
oldXYZ = []
newXYZ = []
# 取原文中的5对点测试
oldXYZ.append((108521, 96611, 101222))
oldXYZ.append((108819, 99931, 101213))
oldXYZ.append((108379, 99937, 101438))
oldXYZ.append((108965, 101930, 101206))
oldXYZ.append((108302, 101930, 101583))
newXYZ.append((0, 0, 0))
newXYZ.append((289, 3327, 0))
newXYZ.append((-144, 3327, 216))
newXYZ.append((444, 5327, 0))
newXYZ.append((-222, 5327, 333))
# 可以输出T R k参数的值
T, R, k = LACC(oldXYZ, newXYZ)
# 计算原坐标在新坐标系下的坐标，以第一个点为例
XA = oldXYZ[0]
x = T + npmulti(R, np.array(XA).reshape((3, 1)), k)
print("T=", T, "\nR=", R, "\nk=", k)
print(x)
The result is as follows.
4. Analysis
First of all, the calculated matrix is similar to the author’s original text, but there is a gap. The reason is that the number of control points is different. Here, only 5 control points are used, and 3 iterations are performed. At the same time, the error in the unit weight of 5 points will be much larger.
Secondly, the actual value of the calculated result is (0,0,0), and the error of the first point is not large. When other points are used, the error may reach 10mm or even 20mm, which belongs to the poor fitting accuracy near that point. If you want to improve the accuracy, you can only add control points.
Finally, emm, in photogrammetry, try not to convert the coordinates as much as possible. After all, the conversion accuracy is a problem. Use relative coordinates as much as possible.
There is another chapter on coordinate transformations.