Import Näherungskoordinaten aus Tachymetermessung in DB

Vorbereitungen_Fabian/Müll/Transformation_Helmert_V1.py

@@ -0,0 +1,141 @@
+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])
+
+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], 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[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])
+A = A_ohne_zahlen.subs(zahlen)
+
+#print(J)
+#print(J_zahlen.evalf(n=3))
+
+# Parameterschätzung
+schwellenwert = 1e-3
+alle_kleiner = True
+
+x0 = sp.Matrix([zahlen[dX], zahlen[dY], zahlen[dZ], zahlen[m], zahlen[q0], zahlen[q1], zahlen[q2], zahlen[q3]])
+P = sp.eye(15)
+N = A.T * P * A
+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!
+n = A.T * P * l
+Qxx = N.evalf(n=30).inv()
+dx = Qxx * n
+x = x0 + dx
+
+
+print(x0.evalf(n=3))
+print(dx.evalf(n=3))
+print(x.evalf(n=3))
+
+for i in range (dx.rows):
+    wert = float(dx[i])
+    if abs(wert) >= schwellenwert:
+        print("Warnung")
+        alle_kleiner = False
+
+if alle_kleiner: print("Ausgleichung fertig!")