fa2338/Masterprojekt-Campusnetz
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Transformationen

Browse Source
This commit is contained in:
fa2338
2025-12-02 12:35:43 +01:00
parent e0f4c24132
commit fd4dca5818
2 changed files with 39 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Vorbereitungen_Fabian/Test.py
View File

@@ -2,7 +2,7 @@ import sympy as sp
dX, dY, dZ, m, q0, q1, q2, q3 = sp.symbols('dX dY dZ m q0 q1 q2 q3') dX, dY, dZ, m, q0, q1, q2, q3 = sp.symbols('dX dY dZ m q0 q1 q2 q3')
f = sp.Matrix([dX - q0 + 1]) f = sp.Matrix([dX + m * q0 + 1])
J = f.jacobian([dX, dY, q0]) J = f.jacobian([dX, dY, q0])

53
Vorbereitungen_Fabian/Transformation_Helmert.py
View File

@@ -48,25 +48,21 @@ else:
print("R ist nicht Orthonormal!") print("R ist nicht Orthonormal!")
# Testmatrix R aus Luhmann S. 66 # Testmatrix R aus Luhmann S. 66
R = sp.Matrix([ #R = sp.Matrix([
[0.996911, -0.013541, -0.077361], # [0.996911, -0.013541, -0.077361],
[0.030706, 0.973820, 0.225238], # [0.030706, 0.973820, 0.225238],
[0.072285, -0.226918, 0.971228] # [0.072285, -0.226918, 0.971228]
]) #])
# 3) Quaternionen berechnen # 3) Quaternionen berechnen
# ToDo: Prüfen, ob Vorzeichen bei q0 richtig ist! # ToDo: Prüfen, ob Vorzeichen bei q0 richtig ist!
#ToDo: q0 stimmt nicht mit Luhmann überein! #ToDo: q0 stimmt nicht mit Luhmann überein!
q0 = 1 / 2 * sp.sqrt(R[0, 0] + R[1, 1] + R[2, 2])
q1 = (R[2, 1] - R[1, 2]) / (4 * q0)
q2 = (R[0, 2] - R[2, 0]) / (4 * q0)
q3 = (R[1, 0] - R[0, 1]) / (4 * q0)
q = Quaternion.from_rotation_matrix(R) q = Quaternion.from_rotation_matrix(R)
q0 = q.a q0_wert = q.a
q1 = q.b q1_wert = q.b
q2 = q.c q2_wert = q.c
q3 = q.d q3_wert = q.d
@@ -79,6 +75,33 @@ for punkt in liste_Punkte:
liste_beobachtungen.append(f"Y_{punkt}") liste_beobachtungen.append(f"Y_{punkt}")
liste_beobachtungen.append(f"Z_{punkt}") liste_beobachtungen.append(f"Z_{punkt}")
print(liste_beobachtungen)
# ToDo: Sympy Funktion jacobian nutzen!
dX, dY, dZ, m, q0, q1, q2, q3, xp1, yp1, zp1 = sp.symbols('dX dY dZ m q0 q1 q2 q3 xp1 yp1 zp1')
#print(Translation[0])
zahlen = {dX: Translation[0], dY: Translation[1], dZ: Translation[2], m: m0, q0: q0_wert, q1: q1_wert, q2: q2_wert, q3: q3_wert, xp1: p1[0], yp1: p1[1], zp1: p1[2]}
#print(zahlen[zp1])
f = sp.Matrix(
[[dX + m * (xp1 * (1 - 2 * (q2**2 + q3**2)) + yp1 * (2 * (q1 * q2 - q0 * q3)) + zp1 * (2 * (q0 * q2 - q1 * q3)))],
[dY + m * (xp1 * (2 * (q1 * q2 + q0 * q3)) + yp1 * (1 - 2 * (q1**2 + q3**2)) + zp1 * (2 * (q2 * q3 - q0 * q1)))],
[dZ + m * (xp1 * (2 * (q1 * q3 - q0 * q2)) + yp1 * (2 * (q0 * q1 + q2 * q3)) + zp1 * (1 - 2 * (q1**2 + q2**2)))]
]
)
A_ohne_zahlen = f.jacobian([dX, dY, dZ, m, q0, q1, q2, q3])
A = A_ohne_zahlen.subs(zahlen)
#print(J)
#print(J_zahlen.evalf(n=3))
# Parameterschätzung
P = sp.eye(3)
N = A.T * P * A
l = sp.Matrix([p1[0], p1[1], p1[2]])
n = A.T * P * l
print(n.evalf(n=3))