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

Helmerttransformation fürs erste fertig, bis GNSS-Daten vorliegen

Browse Source
This commit is contained in:
fa2338
2025-12-09 14:30:28 +01:00
parent 74d467e957
commit 859cbbade9
8 changed files with 2486 additions and 1301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
Vorbereitungen_Fabian/Müll/Helmert_Quaternionen_Müll.py Normal file
View File

@@ -0,0 +1,208 @@
@staticmethod
def R_matrix_aus_quaternion(q0, q1, q2, q3):
return sp.Matrix([
[1 - 2 * (q2 ** 2 + q3 ** 2), 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)],
[2 * (q1 * q2 + q0 * q3), 1 - 2 * (q1 ** 2 + q3 ** 2), 2 * (q2 * q3 - q0 * q1)],
[2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 ** 2 + q2 ** 2)]
])
def Helmerttransformation_Quaternionen(self):
db = Datenbank.Datenbankzugriff(self.pfad_datenbank)
dict_ausgangssystem = db.get_koordinaten("naeherung_lh", "Dict")
dict_zielsystem = db.get_koordinaten("naeherung_us", "Dict")
gemeinsame_punktnummern = sorted(set(dict_ausgangssystem.keys()) & set(dict_zielsystem.keys()))
anzahl_gemeinsame_punkte = len(gemeinsame_punktnummern)
liste_punkte_ausgangssystem = [dict_ausgangssystem[i] for i in gemeinsame_punktnummern]
liste_punkte_zielsystem = [dict_zielsystem[i] for i in gemeinsame_punktnummern]
print("Anzahl gemeinsame Punkte:", anzahl_gemeinsame_punkte)
print("\nErste Zielpunkte:")
for pn, P in list(zip(gemeinsame_punktnummern, liste_punkte_zielsystem))[:5]:
print(pn, [float(P[0]), float(P[1]), float(P[2])])
print("\nErste Ausgangspunkte:")
for pn, p in list(zip(gemeinsame_punktnummern, liste_punkte_ausgangssystem))[:5]:
print(pn, [float(p[0]), float(p[1]), float(p[2])])
# ToDo: Achtung: Die Ergebnisse sind leicht anders, als in den Beispielrechnung von Luhmann (Rundungsfehler bei Luhmann?)
# ToDo: Automatische Ermittlung der Anzahl Nachkommastellen für Test auf Orthonormalität integrieren!
p1, p2, p3 = liste_punkte_ausgangssystem[0], liste_punkte_ausgangssystem[1], liste_punkte_ausgangssystem[2]
P1, P2, P3 = liste_punkte_zielsystem[0], liste_punkte_zielsystem[1], liste_punkte_zielsystem[2]
# 1) Näherungswertberechnung
m0 = (P2 - P1).norm() / (p2 - p1).norm()
U = (P2 - P1) / (P2 - P1).norm()
W = (U.cross(P3 - P1)) / (U.cross(P3 - P1)).norm()
V = W.cross(U)
u = (p2 - p1) / (p2 - p1).norm()
w = (u.cross(p3 - p1)) / (u.cross(p3 - p1)).norm()
v = w.cross(u)
R = sp.Matrix.hstack(U, V, W) * sp.Matrix.hstack(u, v, w).T
XS = (P1 + P2 + P3) / 3
xS = (p1 + p2 + p3) / 3
Translation = XS - m0 * R * xS
# 2) Test auf orthonormale Drehmatrix bei 3 Nachkommastellen!
if R.T.applyfunc(lambda x: round(float(x), 3)) == R.inv().applyfunc(lambda x: round(float(x), 3)) and (
R.T * R).applyfunc(lambda x: round(float(x), 3)) == sp.eye(3).applyfunc(lambda x: round(float(x), 3)) and (
(round(R.det(), 3) == 1.000 or round(R.det(), 3) == -1.000)):
print("R ist Orthonormal!")
else:
print("R ist nicht Orthonormal!")
# 3) Quaternionen berechnen
# ToDo: Prüfen, ob Vorzeichen bei q0 richtig ist!
# ToDo: q0 stimmt nicht mit Luhmann überein!
q = Quaternion.from_rotation_matrix(R)
q0_wert = q.a
q1_wert = q.b
q2_wert = q.c
q3_wert = q.d
dX, dY, dZ, m, q0, q1, q2, q3 = sp.symbols('dX dY dZ m q0 q1 q2 q3')
R_symbolisch = self.R_matrix_aus_quaternion(q0, q1, q2, q3)
# 4) Funktionales Modell
f_zeilen = []
for punkt in liste_punkte_ausgangssystem:
punkt_vektor = sp.Matrix([punkt[0], punkt[1], punkt[2]])
f_zeile_i = sp.Matrix([dX, dY, dZ]) + m * R_symbolisch * punkt_vektor
f_zeilen.extend(list(f_zeile_i))
f_matrix = sp.Matrix(f_zeilen)
f = f_matrix
A_ohne_zahlen = f_matrix.jacobian([dX, dY, dZ, m, q0, q1, q2, q3])
# Parameterschätzung
schwellenwert = 1e-4
anzahl_iterationen = 0
alle_kleiner_vorherige_iteration = False
l_vektor = sp.Matrix([koord for P in liste_punkte_zielsystem for koord in P])
l = l_vektor
P = sp.eye(3 * anzahl_gemeinsame_punkte)
l_berechnet_0 = None
while True:
if anzahl_iterationen == 0:
zahlen_0 = {dX: float(Translation[0]), dY: float(Translation[1]), dZ: float(Translation[2]), m: float(m0),
q0: float(q0_wert), q1: float(q1_wert),
q2: float(q2_wert),
q3: float(q3_wert)}
x0 = sp.Matrix(
[zahlen_0[dX], zahlen_0[dY], zahlen_0[dZ], zahlen_0[m], zahlen_0[q0], zahlen_0[q1], zahlen_0[q2],
zahlen_0[q3]])
l_berechnet_0 = f.subs(zahlen_0).evalf(n=30)
dl_0 = l_vektor - l_berechnet_0
A_0 = A_ohne_zahlen.subs(zahlen_0).evalf(n=30)
N = A_0.T * P * A_0
n_0 = A_0.T * P * dl_0
Qxx_0 = N.inv()
dx = Qxx_0 * n_0
x = x0 + dx
x = sp.N(x, 30) # 30 Nachkommastellen
q_norm = sp.sqrt(x[4] ** 2 + x[5] ** 2 + x[6] ** 2 + x[7] ** 2)
x = sp.Matrix(x)
x[4] /= q_norm
x[5] /= q_norm
x[6] /= q_norm
x[7] /= q_norm
anzahl_iterationen += 1
print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen")
print(dx.evalf(n=3))
else:
zahlen_i = {dX: float(x[0]), dY: float(x[1]), dZ: float(x[2]), m: float(x[3]), q0: float(x[4]),
q1: float(x[5]),
q2: float(x[6]),
q3: float(x[7])}
l_berechnet_i = f.subs(zahlen_i).evalf(n=30)
dl_i = l_vektor - l_berechnet_i
A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=30)
N_i = A_i.T * P * A_i
Qxx_i = N_i.inv()
n_i = A_i.T * P * dl_i
dx = Qxx_i * n_i
x = sp.Matrix(x + dx)
q_norm = sp.sqrt(x[4] ** 2 + x[5] ** 2 + x[6] ** 2 + x[7] ** 2)
x[4] /= q_norm
x[5] /= q_norm
x[6] /= q_norm
x[7] /= q_norm
anzahl_iterationen += 1
print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen")
print(dx.evalf(n=3))
alle_kleiner = True
for i in range(dx.rows):
wert = float(dx[i])
if abs(wert) > schwellenwert:
alle_kleiner = False
if alle_kleiner and alle_kleiner_vorherige_iteration or anzahl_iterationen == 100:
break
alle_kleiner_vorherige_iteration = alle_kleiner
print(l.evalf(n=3))
print(l_berechnet_0.evalf(n=3))
print(f"x = {x.evalf(n=3)}")
# Neuberechnung Zielsystem
zahlen_final = {
dX: float(x[0]),
dY: float(x[1]),
dZ: float(x[2]),
m: float(x[3]),
q0: float(x[4]),
q1: float(x[5]),
q2: float(x[6]),
q3: float(x[7])
}
l_berechnet_final = f.subs(zahlen_final).evalf(n=30)
liste_l_berechnet_final = []
for i in range(anzahl_gemeinsame_punkte):
Xi = l_berechnet_final[3 * i + 0]
Yi = l_berechnet_final[3 * i + 1]
Zi = l_berechnet_final[3 * i + 2]
liste_l_berechnet_final.append(sp.Matrix([Xi, Yi, Zi]))
print("")
print("l_berechnet_final:")
for punktnummer, l_fin in zip(gemeinsame_punktnummern, liste_l_berechnet_final):
print(f"{punktnummer}: {float(l_fin[0]):.3f}, {float(l_fin[1]):.3f}, {float(l_fin[2]):.3f}")
print("Streckendifferenzen:")
streckendifferenzen = [
(punkt_zielsys - l_final).norm()
for punkt_zielsys, l_final in zip(liste_punkte_zielsystem, liste_l_berechnet_final)
]
print([round(float(s), 6) for s in streckendifferenzen])
Schwerpunkt_Zielsystem = sum(liste_punkte_zielsystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
Schwerpunkt_berechnet = sum(liste_l_berechnet_final, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
Schwerpunktsdifferenz = Schwerpunkt_Zielsystem - Schwerpunkt_berechnet
print("\nDifferenz Schwerpunkt (Vektor):")
print(Schwerpunktsdifferenz.evalf(3))
print("Betrag der Schwerpunkt-Differenz:")
print(f"{float(Schwerpunktsdifferenz.norm()):.3f}m")
# ToDo: Abweichungen in Printausgabe ausgeben!

216
Vorbereitungen_Fabian/Transformation_Helmert_V3_final.py
View File

@@ -265,4 +265,218 @@ print(Schwerpunktsdifferenz.evalf(3))
print("Betrag der Schwerpunkt-Differenz:")
print(f"{float(Schwerpunktsdifferenz.norm()):.3f}m")
#ToDo: Abweichungen in Printausgabe ausgeben!
#ToDo: Abweichungen in Printausgabe ausgeben!
def Helmerttransformation_Euler(self):
db = Datenbank.Datenbankzugriff(self.pfad_datenbank)
dict_ausgangssystem = db.get_koordinaten("naeherung_lh", "Dict")
dict_zielsystem = db.get_koordinaten("naeherung_us", "Dict")
gemeinsame_punktnummern = sorted(set(dict_ausgangssystem.keys()) & set(dict_zielsystem.keys()))
anzahl_gemeinsame_punkte = len(gemeinsame_punktnummern)
liste_punkte_ausgangssystem = [dict_ausgangssystem[i] for i in gemeinsame_punktnummern]
liste_punkte_zielsystem = [dict_zielsystem[i] for i in gemeinsame_punktnummern]
print("Anzahl gemeinsame Punkte:", anzahl_gemeinsame_punkte)
print("\nErste Zielpunkte:")
for pn, P in list(zip(gemeinsame_punktnummern, liste_punkte_zielsystem))[:5]:
print(pn, [float(P[0]), float(P[1]), float(P[2])])
print("\nErste Ausgangspunkte:")
for pn, p in list(zip(gemeinsame_punktnummern, liste_punkte_ausgangssystem))[:5]:
print(pn, [float(p[0]), float(p[1]), float(p[2])])
# --- Näherungswerte (minimal erweitert) ---
p1, p2, p3 = liste_punkte_ausgangssystem[0], liste_punkte_ausgangssystem[1], liste_punkte_ausgangssystem[2]
P1, P2, P3 = liste_punkte_zielsystem[0], liste_punkte_zielsystem[1], liste_punkte_zielsystem[2]
# 1) Näherungswert Maßstab: Mittelwert aus allen Punktpaaren
ratios = []
for i, j in combinations(range(anzahl_gemeinsame_punkte), 2):
dp = (liste_punkte_ausgangssystem[j] - liste_punkte_ausgangssystem[i]).norm()
dP = (liste_punkte_zielsystem[j] - liste_punkte_zielsystem[i]).norm()
dp_f = float(dp)
if dp_f > 0:
ratios.append(float(dP) / dp_f)
m0 = sum(ratios) / len(ratios)
if ratios:
print("min/mean/max:",
min(ratios),
sum(ratios) / len(ratios),
max(ratios))
U = (P2 - P1) / (P2 - P1).norm()
W = (U.cross(P3 - P1)) / (U.cross(P3 - P1)).norm()
V = W.cross(U)
u = (p2 - p1) / (p2 - p1).norm()
w = (u.cross(p3 - p1)) / (u.cross(p3 - p1)).norm()
v = w.cross(u)
R0 = sp.Matrix.hstack(U, V, W) * sp.Matrix.hstack(u, v, w).T
XS = sum(liste_punkte_zielsystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
xS = sum(liste_punkte_ausgangssystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
Translation0 = XS - m0 * R0 * xS
# 2) Test auf orthonormale Drehmatrix bei 3 Nachkommastellen!
if R0.T.applyfunc(lambda x: round(float(x), 3)) == R0.inv().applyfunc(lambda x: round(float(x), 3)) \
and (R0.T * R0).applyfunc(lambda x: round(float(x), 3)) == sp.eye(3).applyfunc(lambda x: round(float(x), 3)) \
and ((round(R0.det(), 3) == 1.000 or round(R0.det(), 3) == -1.000)):
print("R ist Orthonormal!")
else:
print("R ist nicht Orthonormal!")
# 3) Euler-Näherungswerte aus R0
e2_0 = sp.asin(R0[2, 0])
# Schutz gegen Division durch 0 wenn cos(e2) ~ 0:
cos_e2_0 = sp.cos(e2_0)
e1_0 = sp.acos(R0[2, 2] / cos_e2_0)
e3_0 = sp.acos(R0[0, 0] / cos_e2_0)
# --- Symbolische Unbekannte (klassische 7 Parameter) ---
dX, dY, dZ, m, e1, e2, e3 = sp.symbols('dX dY dZ m e1 e2 e3')
R_symbolisch = self.R_matrix_aus_euler(e1, e2, e3)
# 4) Funktionales Modell
f_zeilen = []
for punkt in liste_punkte_ausgangssystem:
punkt_vektor = sp.Matrix([punkt[0], punkt[1], punkt[2]])
f_zeile_i = sp.Matrix([dX, dY, dZ]) + m * R_symbolisch * punkt_vektor
f_zeilen.extend(list(f_zeile_i))
f_matrix = sp.Matrix(f_zeilen)
f = f_matrix
A_ohne_zahlen = f_matrix.jacobian([dX, dY, dZ, m, e1, e2, e3])
# Parameterschätzung
schwellenwert = 1e-4
anzahl_iterationen = 0
alle_kleiner_vorherige_iteration = False
l_vektor = sp.Matrix([koord for P in liste_punkte_zielsystem for koord in P])
l = l_vektor
P_mat = sp.eye(3 * anzahl_gemeinsame_punkte)
l_berechnet_0 = None
while True:
if anzahl_iterationen == 0:
zahlen_0 = {
dX: float(Translation0[0]),
dY: float(Translation0[1]),
dZ: float(Translation0[2]),
m: float(m0),
e1: float(e1_0),
e2: float(e2_0),
e3: float(e3_0)
}
x0 = sp.Matrix([zahlen_0[dX], zahlen_0[dY], zahlen_0[dZ],
zahlen_0[m], zahlen_0[e1], zahlen_0[e2], zahlen_0[e3]])
l_berechnet_0 = f.subs(zahlen_0).evalf(n=30)
dl_0 = l_vektor - l_berechnet_0
A_0 = A_ohne_zahlen.subs(zahlen_0).evalf(n=30)
N = A_0.T * P_mat * A_0
n_0 = A_0.T * P_mat * dl_0
Qxx_0 = N.inv()
dx = Qxx_0 * n_0
x = x0 + dx
x = sp.N(x, 30)
anzahl_iterationen += 1
print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen")
print(dx.evalf(n=3))
else:
zahlen_i = {
dX: float(x[0]),
dY: float(x[1]),
dZ: float(x[2]),
m: float(x[3]),
e1: float(x[4]),
e2: float(x[5]),
e3: float(x[6])
}
l_berechnet_i = f.subs(zahlen_i).evalf(n=30)
dl_i = l_vektor - l_berechnet_i
A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=30)
N_i = A_i.T * P_mat * A_i
Qxx_i = N_i.inv()
n_i = A_i.T * P_mat * dl_i
dx = Qxx_i * n_i
x = sp.Matrix(x + dx)
anzahl_iterationen += 1
print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen")
print(dx.evalf(n=3))
alle_kleiner = True
for i in range(dx.rows):
wert = float(dx[i])
if abs(wert) > schwellenwert:
alle_kleiner = False
if (alle_kleiner and alle_kleiner_vorherige_iteration) or anzahl_iterationen == 100:
break
alle_kleiner_vorherige_iteration = alle_kleiner
print(l.evalf(n=3))
print(l_berechnet_0.evalf(n=3))
print(f"x = {x.evalf(n=3)}")
# --- Neuberechnung Zielsystem ---
zahlen_final = {
dX: float(x[0]),
dY: float(x[1]),
dZ: float(x[2]),
m: float(x[3]),
e1: float(x[4]),
e2: float(x[5]),
e3: float(x[6])
}
l_berechnet_final = f.subs(zahlen_final).evalf(n=30)
liste_l_berechnet_final = []
for i in range(anzahl_gemeinsame_punkte):
Xi = l_berechnet_final[3 * i + 0]
Yi = l_berechnet_final[3 * i + 1]
Zi = l_berechnet_final[3 * i + 2]
liste_l_berechnet_final.append(sp.Matrix([Xi, Yi, Zi]))
print("")
print("l_berechnet_final:")
for punktnummer, l_fin in zip(gemeinsame_punktnummern, liste_l_berechnet_final):
print(f"{punktnummer}: {float(l_fin[0]):.3f}, {float(l_fin[1]):.3f}, {float(l_fin[2]):.3f}")
print("Streckendifferenzen:")
streckendifferenzen = [
(punkt_zielsys - l_final).norm()
for punkt_zielsys, l_final in zip(liste_punkte_zielsystem, liste_l_berechnet_final)
]
print([round(float(s), 6) for s in streckendifferenzen])
Schwerpunkt_Zielsystem = sum(liste_punkte_zielsystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
Schwerpunkt_berechnet = sum(liste_l_berechnet_final, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
Schwerpunktsdifferenz = Schwerpunkt_Zielsystem - Schwerpunkt_berechnet
print("\nDifferenz Schwerpunkt (Vektor):")
print(Schwerpunktsdifferenz.evalf(3))
print("Betrag der Schwerpunkt-Differenz:")
print(f"{float(Schwerpunktsdifferenz.norm()):.3f}m")