import sympy as sp from sympy.algebras.quaternion import Quaternion #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! #Beipsiel aus Luhmann S. 76 # Ausgangssystem p1 = sp.Matrix([110, 100, 110]) p2 = sp.Matrix([150, 280, 100]) p3 = sp.Matrix([300, 300, 120]) p4 = sp.Matrix([170, 100, 100]) p5 = sp.Matrix([200, 200, 140]) # Zielsystem P1 = sp.Matrix([153.559, 170.747, 150.768]) P2 = sp.Matrix([99.026, 350.313, 354.912]) P3 = sp.Matrix([215.054, 544.420, 319.003]) P4 = sp.Matrix([179.413, 251.030, 115.601]) P5 = sp.Matrix([213.431, 340.349, 253.036]) #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 #print(m0.evalf()) #print(R.evalf()) #print(Translation.evalf()) # 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!") # Testmatrix R aus Luhmann S. 66 #R = sp.Matrix([ # [0.996911, -0.013541, -0.077361], # [0.030706, 0.973820, 0.225238], # [0.072285, -0.226918, 0.971228] #]) # 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 # 4) Funktionales Modell liste_Punkte = ["P1", "P2", "P3", "P4", "P5"] liste_unbekannte = ["dX", "dY", "dZ", "dm", "dq0", "dq1", "dq2", "dq3"] liste_beobachtungen =[] for punkt in liste_Punkte: liste_beobachtungen.append(f"X_{punkt}") liste_beobachtungen.append(f"Y_{punkt}") liste_beobachtungen.append(f"Z_{punkt}") dX, dY, dZ, m, q0, q1, q2, q3, xp1, yp1, zp1, xp2, yp2, zp2, xp3, yp3, zp3, xp4, yp4, zp4, xp5, yp5, zp5 = sp.symbols('dX dY dZ m q0 q1 q2 q3 xp1 yp1 zp1 xp2 yp2 zp2 xp3 yp3 zp3 xp4 yp4 zp4 xp5 yp5 zp5') #print(Translation[0]) #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)))], [dX + m * (xp2 * (1 - 2 * (q2**2 + q3**2)) + yp2 * (2 * (q1 * q2 - q0 * q3)) + zp2 * (2 * (q0 * q2 + q1 * q3)))], [dY + m * (xp2 * (2 * (q1 * q2 + q0 * q3)) + yp2 * (1 - 2 * (q1**2 + q3**2)) + zp2 * (2 * (q2 * q3 - q0 * q1)))], [dZ + m * (xp2 * (2 * (q1 * q3 - q0 * q2)) + yp2 * (2 * (q0 * q1 + q2 * q3)) + zp2 * (1 - 2 * (q1**2 + q2**2)))], [dX + m * (xp3 * (1 - 2 * (q2**2 + q3**2)) + yp3 * (2 * (q1 * q2 - q0 * q3)) + zp3 * (2 * (q0 * q2 + q1 * q3)))], [dY + m * (xp3 * (2 * (q1 * q2 + q0 * q3)) + yp3 * (1 - 2 * (q1**2 + q3**2)) + zp3 * (2 * (q2 * q3 - q0 * q1)))], [dZ + m * (xp3 * (2 * (q1 * q3 - q0 * q2)) + yp3 * (2 * (q0 * q1 + q2 * q3)) + zp3 * (1 - 2 * (q1**2 + q2**2)))], [dX + m * (xp4 * (1 - 2 * (q2**2 + q3**2)) + yp4 * (2 * (q1 * q2 - q0 * q3)) + zp4 * (2 * (q0 * q2 + q1 * q3)))], [dY + m * (xp4 * (2 * (q1 * q2 + q0 * q3)) + yp4 * (1 - 2 * (q1**2 + q3**2)) + zp4 * (2 * (q2 * q3 - q0 * q1)))], [dZ + m * (xp4 * (2 * (q1 * q3 - q0 * q2)) + yp4 * (2 * (q0 * q1 + q2 * q3)) + zp4 * (1 - 2 * (q1**2 + q2**2)))], [dX + m * (xp5 * (1 - 2 * (q2**2 + q3**2)) + yp5 * (2 * (q1 * q2 - q0 * q3)) + zp5 * (2 * (q0 * q2 + q1 * q3)))], [dY + m * (xp5 * (2 * (q1 * q2 + q0 * q3)) + yp5 * (1 - 2 * (q1**2 + q3**2)) + zp5 * (2 * (q2 * q3 - q0 * q1)))], [dZ + m * (xp5 * (2 * (q1 * q3 - q0 * q2)) + yp5 * (2 * (q0 * q1 + q2 * q3)) + zp5 * (1 - 2 * (q1**2 + q2**2)))], ] ) A_ohne_zahlen = f.jacobian([dX, dY, dZ, m, q0, q1, q2, q3]) #print(J) #print(J_zahlen.evalf(n=3)) # Parameterschätzung schwellenwert = 1e-4 anzahl_iterationen = 0 alle_kleiner_vorherige_iteration = False P = sp.eye(15) #l = sp.Matrix([p1[0], p1[1], p1[2], p2[0], p2[1], p2[2], p3[0], p3[1], p3[2], p4[0], p4[1], p4[2], p5[0], p5[1], p5[2]]) liste_punkte_ausgangssystem = [p1, p2, p3, p4, p5] #l = sp.Matrix([P1[0] - p1[0], P1[1] - p1[1], P1[2] - p1[2], P2[0] - p2[0], P2[1] - p2[1], P2[2] - p2[2], P3[0] - p3[0], P3[1] - p3[1], P3[2] - p3[2], P4[0] - p4[0], P4[1] - p4[1], P4[2] - p4[2], P5[0] - p5[0], P5[1] - p5[1], P5[2] - p5[2]]) l = sp.Matrix([P1[0], P1[1], P1[2], P2[0], P2[1], P2[2], P3[0], P3[1], P3[2], P4[0], P4[1], P4[2], P5[0], P5[1], P5[2]]) # ToDo: Prüfen, ob n mit l oder mit dl! 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), xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} 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]]) R_matrix_0 = sp.Matrix([[1 - 2 * (q2_wert ** 2 + q3_wert ** 2), 2 * (q1_wert * q2_wert - q0_wert * q3_wert), 2 * (q0_wert * q2_wert + q1_wert * q3_wert)], [2 * (q1_wert * q2_wert + q0_wert * q3_wert), 1 - 2 * (q1_wert ** 2 + q3_wert ** 2), 2 * (q2_wert * q3_wert - q0_wert * q1_wert)], [2 * (q1_wert * q3_wert - q0_wert * q2_wert), 2 * (q0_wert * q1_wert + q2_wert * q3_wert), 1 - 2 * (q1_wert ** 2 + q2_wert ** 2)]]) liste_l_berechnet_0 = [Translation + m0 * R_matrix_0 * p for p in liste_punkte_ausgangssystem] l_berechnet_0 = sp.Matrix.vstack(*liste_l_berechnet_0) dl_0 = l - l_berechnet_0 A_0 = A_ohne_zahlen.subs(zahlen_0) N = A_0.T * P * A_0 n_0 = A_0.T * P * dl_0 Qxx_0 = N.evalf(n=30).inv() dx = Qxx_0 * n_0 x = x0 + dx x = sp.N(x, 10) # 10 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]), xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} R_matrix_i = sp.Matrix([[1 - 2 * (zahlen_i[q2] ** 2 + zahlen_i[q3] ** 2), 2 * (zahlen_i[q1] * zahlen_i[q2] - zahlen_i[q0] * zahlen_i[q3]), 2 * (zahlen_i[q0] * zahlen_i[q2] + zahlen_i[q1] * zahlen_i[q3])], [2 * (zahlen_i[q1] * zahlen_i[q2] + zahlen_i[q0] * zahlen_i[q3]), 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q3] ** 2), 2 * (zahlen_i[q2] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q1])], [2 * (zahlen_i[q1] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q2]), 2 * (zahlen_i[q0] * zahlen_i[q1] + zahlen_i[q2] * zahlen_i[q3]), 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q2] ** 2)]]) #print("R_matrix_i") liste_l_berechnet_i = [sp.Matrix([zahlen_i[dX], zahlen_i[dY], zahlen_i[dZ]]) + zahlen_i[m] * R_matrix_i * p for p in liste_punkte_ausgangssystem] #print("liste_l_berechnet_i") l_berechnet_i = sp.Matrix.vstack(*liste_l_berechnet_i) #print("l_berechnet_i") dl_i = l - l_berechnet_i #print("dl_i") A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=30) #print("A_i") N_i = A_i.T * P * A_i #print("N_i") n_i = A_i.T * P * dl_i # print("n_i") Qxx_i = N_i.evalf(n=30).inv() #print("Qxx_i") n_i = A_i.T * P * dl_i #print("n_i") dx = Qxx_i * n_i #print("dx") x += dx x = sp.Matrix(x) 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 # print("x") 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 == 20: 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_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]), xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} # print("zahlen_i") R_matrix_i = sp.Matrix( [[1 - 2 * (zahlen_i[q2] ** 2 + zahlen_i[q3] ** 2), 2 * (zahlen_i[q1] * zahlen_i[q2] - zahlen_i[q0] * zahlen_i[q3]), 2 * (zahlen_i[q0] * zahlen_i[q2] + zahlen_i[q1] * zahlen_i[q3])], [2 * (zahlen_i[q1] * zahlen_i[q2] + zahlen_i[q0] * zahlen_i[q3]), 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q3] ** 2), 2 * (zahlen_i[q2] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q1])], [2 * (zahlen_i[q1] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q2]), 2 * (zahlen_i[q0] * zahlen_i[q1] + zahlen_i[q2] * zahlen_i[q3]), 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q2] ** 2)]]) # print("R_matrix_i") liste_l_berechnet_i = [sp.Matrix([zahlen_i[dX], zahlen_i[dY], zahlen_i[dZ]]) + zahlen_i[m] * R_matrix_i * p for p in liste_punkte_ausgangssystem] # print("liste_l_berechnet_i") l_berechnet_i = sp.Matrix.vstack(*liste_l_berechnet_i) print("") print("l_berechnet_final:") liste_punkte_zielsystem = [P1, P2, P3, P4, P5] for v in l_berechnet_i: print(f"{float(v):.3f}") print("Streckendifferenzen:") streckendifferenzen = [(P - L).norm() for P, L in zip(liste_punkte_zielsystem, liste_l_berechnet_i)] print([round(float(s), 6) for s in streckendifferenzen]) Schwerpunkt_Zielsystem = sum(liste_punkte_zielsystem, sp.Matrix([0, 0, 0])) / len(liste_punkte_zielsystem) Schwerpunkt_berechnet = sum(liste_l_berechnet_i, sp.Matrix([0, 0, 0])) / len(liste_l_berechnet_i) 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! 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")