fa2338/Masterprojekt_V3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

enabnahem

Browse Source
This commit is contained in:
mi2847
2026-02-11 19:47:07 +01:00
parent 8b928c1bb7
commit 5749d173a0
12 changed files with 13284 additions and 14884 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26506
Ausgleichungsprogramm.ipynb
View File

File diff suppressed because it is too large Load Diff

3
Export.py
View File

@@ -252,15 +252,18 @@ class Export:
{ergebnisse['df_aeussere_zuver'].to_html(index=False)}
<h3 id="Helmertscher_Punktfehler">6. Standardabweichungen und Helmert'scher Punktfehler</h3>
<p><strong>Mittlerer Punktfehler über alle Punkte:</strong> {ergebnisse['mittlerer_punktfehler']:.6f} m</p>
{ergebnisse['df_punktfehler'].to_html(index=False)}
<h3 id="Standardellipsoid">7. Standardellipsoid</h3>
{ergebnisse['df_standardellipsoid'].to_html(index=False)}
<h3 id="Konfidenzellipoid">8. Konfidenzellipoid</h3>
<p><strong>Konfidenzniveau:</strong> {(1 - ergebnisse['alpha_konf']):.1%}</p>
{ergebnisse['df_konfidenzellipsoid'].to_html(index=False)}
<h3 id="Konfidenzellipse_ENU">9. Konfidenzellipsen im ENU-System</h3>
<p><strong>Konfidenzniveau:</strong> {(1- ergebnisse['alpha_konf2d']):.1%}</p>
{ergebnisse['df_konfidenzellipsen_enu'].to_html(index=False)}
<h3 id="netzplot_gesamt">10. Plots: Konfidenzellipsen im ENU-System (Gesamtes Netz)</h3>

40
Netzqualitaet_Genauigkeit.py
View File

@@ -107,12 +107,12 @@ class Genauigkeitsmaße:
daten.append([pid, float(sx), float(sy), float(sz), float(sP)])
except:
continue
helmert_punktfehler = pd.DataFrame(daten, columns=["Punkt", "σ̂x", "σ̂y", "σ̂z", f"σ̂P_{dim}D"])
mittel_sP = helmert_punktfehler[f"σ̂P_{dim}D"].mean()
print(f"Mittlerer Helmert-Punktfehler über alle Punkte: {mittel_sP:.6f} [m]")
helmert_punktfehler = pd.DataFrame(daten, columns=["Punkt", "σ̂x [m]", "σ̂y [m]", "σ̂z [m]", f"σ̂P_{dim}D [m]"])
mittel_sP = helmert_punktfehler[f"σ̂P_{dim}D [m]"].mean()
print(f"Mittlerer Helmert-Punktfehler über alle Punkte: {mittel_sP:.6f} m")
display(HTML(helmert_punktfehler.to_html(index=False)))
helmert_punktfehler.to_excel(r"Netzqualitaet\Standardabweichungen_Helmertscher_Punktfehler.xlsx",index=False)
return helmert_punktfehler
return helmert_punktfehler, mittel_sP
@@ -255,10 +255,10 @@ class Genauigkeitsmaße:
"""
alpha_input = input("Irrtumswahrscheinlichkeit α wählen (z.B. 0.05, 0.01) [Standard=0.05]: ")
if alpha_input.strip() == "":
alpha = 0.05
alpha_konf = 0.05
else:
alpha = float(alpha_input)
print(f"→ Verwende alpha = {alpha} (Konfidenz = {(1 - alpha) * 100:.1f}%)")
alpha_konf = float(alpha_input)
print(f"→ Verwende alpha = {alpha_konf} (Konfidenz = {(1 - alpha_konf) * 100:.1f}%)")
Qxx = np.asarray(Qxx, float)
daten = []
namen_str = [str(sym) for sym in unbekannten_liste]
@@ -266,7 +266,7 @@ class Genauigkeitsmaße:
punkt_ids = [n[1:] for n in namen_str if n.upper().startswith('X')]
# Faktor fürs Konfidenzellipoid (F-Verteilung)
k = float(np.sqrt(3.0 * f.ppf(1.0 - alpha, 3, R)))
k = float(np.sqrt(3.0 * f.ppf(1.0 - alpha_konf, 3, R)))
for pid in punkt_ids:
try:
@@ -327,12 +327,12 @@ class Genauigkeitsmaße:
konfidenzellipoid["γ [gon]"] = konfidenzellipoid["γ [gon]"].astype(float).round(3)
display(HTML(konfidenzellipoid.to_html(index=False)))
konfidenzellipoid.to_excel(r"Netzqualitaet\Konfidenzellipoid.xlsx", index=False)
return konfidenzellipoid, alpha
return konfidenzellipoid, alpha_konf
@staticmethod
def konfidenzellipsen(Qxx: np.ndarray, sigma0_apost: float, unbekannten_liste: list, r_gesamt: int) -> pd.DataFrame:
def konfidenzellipsen(Qxx: np.ndarray, sigma0_apost: float, unbekannten_liste: list, r_gesamt: int) -> tuple[pd.DataFrame, float]:
"""
Berechnet Konfidenzellipsen für Punkte aus der Kofaktor-Matrix Qxx.
@@ -355,16 +355,16 @@ class Genauigkeitsmaße:
:param r_gesamt: Redundanz (Freiheitsgrade) des Netzes.
:type r_gesamt: int
:return: Tabelle der Konfidenzellipsen je Punkt.
:rtype: pd.DataFrame
:rtype: tuple[pd.DataFrame, float]
"""
# Irrtumswahrscheinlichkeit alpha
alpha_input = input("Irrtumswahrscheinlichkeit α wählen (z.B. 0.05, 0.01) [Standard=0.05]: ")
if alpha_input.strip() == "":
alpha = 0.05
alpha_konf2d = 0.05
else:
alpha = float(alpha_input)
print(f"→ Verwende alpha = {alpha} (Konfidenz = {(1 - alpha) * 100:.1f}%)")
alpha_konf2d = float(alpha_input)
print(f"→ Verwende alpha = {alpha_konf2d} (Konfidenz = {(1 - alpha_konf2d) * 100:.1f}%)")
Qxx = np.asarray(Qxx, float)
daten = []
@@ -373,7 +373,7 @@ class Genauigkeitsmaße:
punkt_ids = [n[1:] for n in namen_str if n.upper().startswith("X")]
# Faktor für die Konfidenzellipe (F-Verteilung)
k2 = float(np.sqrt(2.0 * f.ppf(1.0 - alpha, 2, r_gesamt)))
k2 = float(np.sqrt(2.0 * f.ppf(1.0 - alpha_konf2d, 2, r_gesamt)))
for pid in punkt_ids:
try:
@@ -411,12 +411,12 @@ class Genauigkeitsmaße:
"Punkt", "σ̂x [m]", "σ̂y [m]",
"Halbachse a_k [m]", "Halbachse b_k [m]", "θ [gon]"
])
return Konfidenzellipse
return Konfidenzellipse, alpha_konf2d
@staticmethod
def konfidenzellipsen_enu(a: float, b: float, ausgabe_parameterschaetzung: dict, liste_unbekannte: list, ausgleichungsergebnis: dict, sigma0apost: float, r_gesamt: int) -> tuple[pd.DataFrame, np.ndarray]:
def konfidenzellipsen_enu(a: float, b: float, ausgabe_parameterschaetzung: dict, liste_unbekannte: list, ausgleichungsergebnis: dict, sigma0apost: float, r_gesamt: int) -> tuple[pd.DataFrame, np.ndarray, float]:
"""
Berechnet Konfidenzellipsen im lokalen ENU-System aus einer ins ENU-System transformierten Qxx-Matrix.
@@ -442,7 +442,7 @@ class Genauigkeitsmaße:
:param r_gesamt: Redundanz (Freiheitsgrade) für die Konfidenzberechnung.
:type r_gesamt: int
:return: Tabelle der Konfidenzellipse im ENU-System, Rotationsmatrix R0 der ENU-Transformation.
:rtype: tuple[pandas.DataFrame, numpy.ndarray]
:rtype: tuple[pandas.DataFrame, numpy.ndarray, float]
"""
berechnungen = Berechnungen.Berechnungen(a, b)
@@ -459,7 +459,7 @@ class Genauigkeitsmaße:
f"ENU-Referenz (Schwerpunkt): B0={Einheitenumrechnung.Einheitenumrechnung.rad_to_gon_Decimal(B0):.8f} rad, L0={Einheitenumrechnung.Einheitenumrechnung.rad_to_gon_Decimal(L0):.8f} rad")
# 2) Konfidenzellipsen im ENU-System
Konfidenzellipse_ENU = Genauigkeitsmaße.konfidenzellipsen(
Konfidenzellipse_ENU, alpha_konf2d = Genauigkeitsmaße.konfidenzellipsen(
Qxx_enu,
sigma0apost,
liste_unbekannte,
@@ -486,7 +486,7 @@ class Genauigkeitsmaße:
# 5) Export
Konfidenzellipse_ENU.to_excel(r"Netzqualitaet\Konfidenzellipse_ENU.xlsx", index=False)
return Konfidenzellipse_ENU, R0
return Konfidenzellipse_ENU, R0, alpha_konf2d

10
Netzqualitaet_Zuverlaessigkeit.py
View File

@@ -385,7 +385,7 @@ class Zuverlaessigkeit:
:param unbekannten_liste: Liste der Unbekannten.
:type unbekannten_liste: list
:param x: Unbekanntenvektor.
:type x: array_like
:type x: numpy.ndarray
:param ausschliessen: Präfixe von Beobachtungsbezeichnungen, die aus der Auswertung entfernt werden sollen
(Standard: ("lA_",) für Lagerungs-/Pseudobeobachtungen).
:type ausschliessen: tuple
@@ -551,11 +551,11 @@ class Zuverlaessigkeit:
"Stand-Pkt": standpunkte,
"Ziel-Pkt": zielpunkte,
"r_i": ri,
"EP_GF [mm]": EP_hat_m * 1000.0,
"EP_grzw [mm]": EP_grzw_m * 1000.0,
"EP_GF [m]": EP_hat_m,
"EP_grzw [m]": EP_grzw_m,
"EF": EF,
"SP_3D [mm]": SP_m * 1000.0,
"EF*SP_3D [mm]": EF_SP_m * 1000.0,
"SP_3D [m]": SP_m,
"EF*SP_3D [m]": EF_SP_m,
})
display(HTML(aeussere_zuverlaessigkeit.to_html(index=False)))
aeussere_zuverlaessigkeit.to_excel(r"Netzqualitaet\Aeussere_Zuverlaessigkeit.xlsx", index=False)

56
Tests_Michelle/Parameterschaetzung_müll.py
View File

@@ -1,56 +0,0 @@
from typing import Dict, Any
import sympy as sp
from Stochastisches_Modell import StochastischesModell
def iterative_ausgleichung(
A: sp.Matrix,
dl: sp.Matrix,
modell: StochastischesModell,
) -> Dict[str, Any]:
ergebnisse_iter = [] #Liste für Zwischenergebnisse
for it in range(max_iter):
Q_ll, P = modell.berechne_Qll_P() #Stochastisches Modell: Qll und P berechnen
N = A.T * P * A #Normalgleichungsmatrix N
Q_xx = N.inv() #Kofaktormatrix der Unbekannten Qxx
n = A.T * P * dl #Absolutgliedvektor n
dx = N.LUsolve(n) #Zuschlagsvektor dx
v = dl - A * dx #Residuenvektor v
Q_vv = modell.berechne_Qvv(A, P, Q_xx) #Kofaktormatrix der Verbesserungen Qvv
R = modell.berechne_R(Q_vv, P) #Redundanzmatrix R
r = modell.berechne_r(R) #Redundanzanteile als Vektor r
ergebnisse_iter.append({ #Zwischenergebnisse speichern in Liste
"iter": it + 1,
"Q_ll": Q_ll,
"P": P,
"N": N,
"Q_xx": Q_xx,
"dx": dx,
"v": v,
"Q_vv": Q_vv,
"R": R,
"r": r,
"sigma_hat": sigma_hat,
"sigma0_groups": dict(modell.sigma0_groups),
})
return {
"dx": dx,
"v": v,
"Q_ll": Q_ll,
"P": P,
"N": N,
"Q_xx": Q_xx,
"Q_vv": Q_vv,
"R": R,
"r": r,
"sigma_hat": sigma_hat,
"sigma0_groups": dict(modell.sigma0_groups),
"history": ergebnisse_iter,
}

49
Vorbereitungen_Fabian/Datenbank_alt.py
View File

@@ -1,49 +0,0 @@
import os
import sqlite3
class Datenbank_anlegen:
def __init__(self, pfad_datenbank):
self.pfad_datenbank = pfad_datenbank
self.db_anlegen()
def db_anlegen(self):
#pfad = r"C:\Users\fabia\OneDrive\Jade HS\Master\MGW2\Masterprojekt_allgemein\Masterprojekt\Programmierung\Campusnetz\Campusnetz.db"
if not os.path.exists(self.pfad_datenbank):
con = sqlite3.connect(self.pfad_datenbank)
cursor = con.cursor()
cursor.executescript("""CREATE TABLE Netzpunkte (
punktnummer TEXT(10),
naeherungx NUMERIC(9,3),
naeherungy NUMERIC(7,3),
naeherungz NUMERIC(8,3),
CONSTRAINT pk_Netzpunkte PRIMARY KEY (punktnummer)
);
CREATE TABLE Standpunkte_Tachymeter (
spID INTEGER PRIMARY KEY AUTOINCREMENT,
punktnummer TEXT(10),
orientierunghz NUMERIC(2,5),
orientierungv NUMERIC(2,5),
dateipfad TEXT(150),
standpunktsnummer INTEGER,
CONSTRAINT fk_Standpunkte_Tachymeter_Netzpunkte FOREIGN KEY (punktnummer)
REFERENCES Netzpunkte(punktnummer)
);
CREATE TABLE Beobachtungen_Tachymeter (
btID INTEGER PRIMARY KEY AUTOINCREMENT,
spID INTEGER,
punktnummer TEXT(10),
hz NUMERIC(3,5),
v NUMERIC(3,5),
distanz NUMERIC(4,4),
CONSTRAINT fk_Beobachtungen_Tachymeter_Standpunkte_Tachymeter FOREIGN KEY (spID)
REFERENCES Standpunkte_Tachymeter(spID),
CONSTRAINT fk_Beobachtungen_Tachymeter_Netzpunkte FOREIGN KEY (punktnummer)
REFERENCES Netzpunkte(punktnummer)
);
""");
con.commit()
con.close()

157
Vorbereitungen_Fabian/Import_Tachymeter.py
View File

@@ -1,157 +0,0 @@
from pathlib import Path
import sqlite3
from decimal import Decimal, getcontext
# ToDo: instrumentenID von Anwender übergeben lassen!
def string_to_decimal(zahl):
zahl = zahl.replace(',', '.')
return Decimal(zahl)
pfad_script = Path(__file__).resolve().parent
dateiname = "campsnetz_beobachtungen_bereinigt.csv"
pfad_datei = pfad_script.parent / "Daten" / dateiname
# Prüfen, ob Bereits Daten aus der Datei in der Datenbank vorhanden sind
pfad_datenbank = pfad_script.parent / "Campusnetz.db"
instrumentenID = 1
con = sqlite3.connect(pfad_datenbank)
cursor = con.cursor()
liste_dateinamen_in_db = [r[0] for r in cursor.execute(
"SELECT DISTINCT dateiname FROM Beobachtungen"
).fetchall()]
liste_beobachtungsgruppeID = [r[0] for r in cursor.execute("""SELECT DISTINCT beobachtungsgruppeID FROM Beobachtungen""").fetchall()]
liste_instrumentenid = [r[0] for r in cursor.execute("SELECT instrumenteID FROM Instrumente").fetchall()]
con.close()
cursor.close
Import_fortsetzen = True
if dateiname in liste_dateinamen_in_db:
Import_fortsetzen = False
if Import_fortsetzen:
nummer_zielpunkt = 0
try:
nummer_beobachtungsgruppeID = max(liste_beobachtungsgruppeID)
except:
nummer_beobachtungsgruppeID = 0
with (open(pfad_datei, "r", encoding="utf-8") as f):
liste_fehlerhafte_zeile = []
liste_beobachtungen_vorbereitung = []
for i, zeile in enumerate(f):
if i < 3:
continue
zeile = zeile.strip().split(";")
if zeile[1] == "" and zeile[2] == "" and zeile[3] == "":
nummer_beobachtungsgruppeID += 1
#print("Standpunkt: ",nummer_beobachtungsgruppeID ,zeile[0])
standpunkt = zeile[0]
if nummer_zielpunkt % 6 != 0:
liste_fehlerhafte_zeile.append(i)
nummer_zielpunkt = 0
liste_zielpunkte_hs = []
liste_zielpunkte_vs2 = []
liste_zielpunkte_vs3 = []
else:
nummer_zielpunkt += 1
if zeile[0] not in liste_zielpunkte_hs:
liste_zielpunkte_hs.append(zeile[0])
if zeile[0] in liste_zielpunkte_vs3:
#print(f"{nummer_zielpunkt} VS3 HS1 {zeile}")
liste_beobachtungen_vorbereitung.append([nummer_beobachtungsgruppeID,"VS3", "HS1", standpunkt, zeile[0], zeile[1], zeile[2], zeile[3]])
elif zeile[0] in liste_zielpunkte_vs2:
#print(f"{nummer_zielpunkt} VS2 HS1 {zeile}")
liste_beobachtungen_vorbereitung.append([nummer_beobachtungsgruppeID,"VS2", "HS1", standpunkt, zeile[0], zeile[1], zeile[2], zeile[3]])
else:
#print(f"{nummer_zielpunkt} VS1 HS1 {zeile}")
liste_beobachtungen_vorbereitung.append(
[nummer_beobachtungsgruppeID,"VS1", "HS1", standpunkt, zeile[0], zeile[1], zeile[2],
zeile[3]])
else:
liste_zielpunkte_hs.remove(zeile[0])
if zeile[0] in liste_zielpunkte_vs3:
#print(f"{nummer_zielpunkt} VS3 HS2 {zeile}")
liste_beobachtungen_vorbereitung.append(
[nummer_beobachtungsgruppeID,"VS3", "HS2", standpunkt, zeile[0], zeile[1], zeile[2],
zeile[3]])
elif zeile[0] in liste_zielpunkte_vs2:
if zeile[0] not in liste_zielpunkte_vs3:
liste_zielpunkte_vs3.append(zeile[0])
#print(f"{nummer_zielpunkt} VS2 HS2 {zeile}")
liste_beobachtungen_vorbereitung.append(
[nummer_beobachtungsgruppeID,"VS2", "HS2", standpunkt, zeile[0], zeile[1], zeile[2],
zeile[3]])
else:
if zeile[0] not in liste_zielpunkte_vs2:
liste_zielpunkte_vs2.append(zeile[0])
#print(f"{nummer_zielpunkt} VS1 HS2 {zeile}")
liste_beobachtungen_vorbereitung.append(
[nummer_beobachtungsgruppeID,"VS1", "HS2", standpunkt, zeile[0], zeile[1], zeile[2],
zeile[3]])
if liste_fehlerhafte_zeile == []:
#print(f"Einlesen der Datei {pfad_datei} erfolgreich beendet.")
pass
else:
print(f"Das Einlesen der Datei {pfad_datei} wurde abgebrochen.\nBitte bearbeiten Sie die Zeilen rund um: {", ".join(map(str, liste_fehlerhafte_zeile))} in der csv-Datei und wiederholen Sie den Import.")
Import_fortsetzen = False
else:
print(f"Der Import wurde abgebrochen, weil die Beobachtungen aus der Datei {pfad_datei} bereits in der Datenbank vorhanden sind.")
if Import_fortsetzen:
liste_beobachtungen_import = []
while len(liste_beobachtungen_vorbereitung) > 0:
liste_aktueller_zielpunkt = liste_beobachtungen_vorbereitung[0]
aktueller_zielpunkt = liste_aktueller_zielpunkt[4]
#print(liste_beobachtungen_vorbereitung[0])
for index in range(1, len(liste_beobachtungen_vorbereitung)):
liste = liste_beobachtungen_vorbereitung[index]
if liste[4] == aktueller_zielpunkt:
#print(liste)
richtung1 = string_to_decimal(liste_aktueller_zielpunkt[5])
richtung2 = string_to_decimal(liste[5]) - Decimal(200)
zenitwinkel_vollsatz = (string_to_decimal(liste_aktueller_zielpunkt[6]) - string_to_decimal(liste[6]) + 400) / 2
distanz_vollsatz = (string_to_decimal(liste_aktueller_zielpunkt[7]) + string_to_decimal(liste[7])) / 2
if richtung2 < 0:
richtung2 += Decimal(400)
elif richtung2 > 400:
richtung2 -= Decimal(400)
richtung_vollsatz = (richtung1 + richtung2) / 2
#print(richtung_vollsatz)
#print(zenitwinkel_vollsatz)
#print(distanz_vollsatz)
liste_beobachtungen_import.append([liste[0], liste[3], liste[4], richtung_vollsatz, zenitwinkel_vollsatz, distanz_vollsatz])
del liste_beobachtungen_vorbereitung[index]
del liste_beobachtungen_vorbereitung[0]
break
if instrumentenID not in liste_instrumentenid:
Import_fortsetzen = False
print("Der Import wurde abgebrochen. Bitte eine gültige InstrumentenID eingeben. Bei Bedarf ist das Instrument neu anzulegen.")
if Import_fortsetzen:
con = sqlite3.connect(pfad_datenbank)
cursor = con.cursor()
for beobachtung_import in liste_beobachtungen_import:
cursor.execute("INSERT INTO Beobachtungen (punktnummer_sp, punktnummer_zp, instrumenteID, beobachtungsgruppeID, tachymeter_richtung, tachymeter_zenitwinkel, tachymeter_distanz, dateiname) VALUES (?, ?, ?, ?, ?, ?, ?, ?)",
(beobachtung_import[1], beobachtung_import[2], instrumentenID, beobachtung_import[0], float(beobachtung_import[3]), float(beobachtung_import[4]), float(beobachtung_import[5]), dateiname))
con.commit()
cursor.close()
con.close()
print(f"Der Import der Datei {pfad_datei} wurde erfolgreich abgeschlossen.")

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

@@ -1,208 +0,0 @@
@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!

141
Vorbereitungen_Fabian/Müll/Transformation_Helmert_V1.py
View File

@@ -1,141 +0,0 @@
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!")

155
Vorbereitungen_Fabian/Müll/Transformation_Helmert_V2.py
View File

@@ -1,155 +0,0 @@
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]])
R_matrix = 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_punkte_ausgangssystem = [p1, p2, p3, p4, p5]
liste_l_berechnet_0 = [Translation + m0 * R_matrix * p for p in liste_punkte_ausgangssystem]
l_berechnet_0 = sp.Matrix.vstack(*liste_l_berechnet_0)
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]])
# 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(l.evalf(n=3))
print(l_berechnet_0.evalf(n=3))
#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!")

361
Vorbereitungen_Fabian/Test.py
View File

@@ -1,361 +0,0 @@
import csv
import xml.etree.ElementTree as ET
from decimal import Decimal, getcontext, ROUND_HALF_UP
# ========= Einstellungen =========
JXL_IN = r"C:\Users\fabia\Desktop\Masterprojekt_V3\Daten\campusnetz_bereinigt_plus_nachmessung.jxl"
CSV_IN = r"C:\Users\fabia\Desktop\Masterprojekt_V3\Daten\campsnetz_beobachtungen_plus_nachmessungen.csv"
CSV_OUT = r"C:\Users\fabia\Desktop\Masterprojekt_V3\Daten\campsnetz_beobachtungen_plus_nachmessungen_korrigiert.csv"
getcontext().prec = 70
# ========= Hilfsfunktionen =========
def count_decimals(s: str, sep: str) -> int:
if s is None:
return 0
s = s.strip()
if s == "":
return 0
if ":ZH:" in s:
s = s.split(":ZH:", 1)[0].strip()
if sep in s:
return len(s.split(sep, 1)[1])
return 0
def fmt_decimal_fixed(x: Decimal, decimals: int, sep: str) -> str:
q = Decimal("1") if decimals == 0 else Decimal("1." + ("0" * decimals))
y = x.quantize(q, rounding=ROUND_HALF_UP)
txt = format(y, "f")
if sep != ".":
txt = txt.replace(".", sep)
if decimals == 0:
txt = txt.split(sep)[0]
return txt
def parse_decimal_csv(s: str) -> Decimal:
"""
CSV-Zahlen mit Komma, evtl. mit ":ZH:..." im letzten Feld.
"""
s = (s or "").strip()
if ":ZH:" in s:
s = s.split(":ZH:", 1)[0].strip()
s = s.replace(",", ".")
return Decimal(s)
def parse_decimal_comma(s: str) -> Decimal:
"""
Komma-String nach Decimal.
"""
return Decimal((s or "").strip().replace(",", "."))
def deg_to_gon_str(deg_str: str) -> str:
"""
JXL: Winkel in Grad (Dezimalpunkt).
gon = deg * (10/9)
Ausgabe mit exakt so vielen Nachkommastellen wie im JXL-Gradwert enthalten.
Dezimaltrennzeichen: Komma.
"""
deg_str = (deg_str or "").strip()
d = count_decimals(deg_str, ".")
deg = Decimal(deg_str)
gon = deg * (Decimal(10) / Decimal(9))
return fmt_decimal_fixed(gon, d, ",")
def meter_str_from_jxl(m_str: str) -> str:
"""
JXL: Distanz in Meter (Dezimalpunkt).
Ausgabe mit exakt so vielen Nachkommastellen wie in der JXL enthalten.
Dezimaltrennzeichen: Komma.
"""
m_str = (m_str or "").strip()
d = count_decimals(m_str, ".")
return fmt_decimal_fixed(Decimal(m_str), d, ",")
def is_obs_line(row: list[str]) -> bool:
"""
Beobachtungszeile: Zielpunkt nicht leer, Hz/Z/SD numerisch parsebar.
Zielpunkt darf alphanumerisch sein (FH3 etc.).
"""
if len(row) < 4:
return False
if row[0].strip() == "" or row[1].strip() == "" or row[2].strip() == "" or row[3].strip() == "":
return False
try:
_ = parse_decimal_csv(row[1])
_ = parse_decimal_csv(row[2])
_ = parse_decimal_csv(row[3])
return True
except Exception:
return False
def is_station_candidate(row: list[str]) -> bool:
"""
Kandidat für Standpunkt: erste Spalte nicht leer, Messspalten leer.
Ob es wirklich ein Standpunkt ist, entscheiden wir später über StationName-Menge.
"""
if len(row) < 4:
return False
return (
row[0].strip() != ""
and row[1].strip() == ""
and row[2].strip() == ""
and row[3].strip() == ""
)
def csv_is_rounding_of_jxl(csv_str: str, jxl_full_str: str) -> bool:
"""
Prüft: CSV ist gerundete Darstellung des JXL-Wertes.
Kriterium:
- CSV hat weniger Nachkommastellen als JXL
- und: JXL auf CSV-Dezimalstellen gerundet == CSV-Wert (numerisch)
"""
dc = count_decimals(csv_str, ",")
dj = count_decimals(jxl_full_str, ",")
if dc >= dj:
return False
try:
csv_val = parse_decimal_csv(csv_str)
jxl_val = parse_decimal_comma(jxl_full_str)
q = Decimal("1") if dc == 0 else Decimal("1." + ("0" * dc))
jxl_rounded = jxl_val.quantize(q, rounding=ROUND_HALF_UP)
csv_q = csv_val.quantize(q, rounding=ROUND_HALF_UP)
return jxl_rounded == csv_q
except Exception:
return False
# ========= JXL einlesen =========
tree = ET.parse(JXL_IN)
root = tree.getroot()
# StationRecords: (StationName, StationID, IH)
station_records = []
for sr in root.iter("StationRecord"):
sname = (sr.findtext("StationName") or "").strip()
sid = (sr.attrib.get("ID") or "").strip()
ih = (sr.findtext("TheodoliteHeight") or "").strip()
if sname != "" and sid != "":
station_records.append((sname, sid, ih))
station_names_set = {sname for sname, _, _ in station_records}
# pro StationName ggf. mehrere Aufbauten -> "nächsten unbenutzten" nehmen
stationname_to_records = {}
for sname, sid, ih in station_records:
stationname_to_records.setdefault(sname, []).append((sid, ih))
stationname_usecount = {k: 0 for k in stationname_to_records.keys()}
# TargetHeight je TargetRecord-ID
target_height_by_id = {}
for tr in root.iter("TargetRecord"):
tid = (tr.attrib.get("ID") or "").strip()
zh = (tr.findtext("TargetHeight") or "").strip()
if tid != "":
target_height_by_id[tid] = zh
# Pro StationID: Sequenz der PointRecords
station_seq = {sid: [] for _, sid, _ in station_records}
for pr in root.iter("PointRecord"):
stid = (pr.findtext("StationID") or "").strip()
if stid == "" or stid not in station_seq:
continue
circle = pr.find("Circle")
if circle is None:
continue
target_name = (pr.findtext("Name") or "").strip()
target_id = (pr.findtext("TargetID") or "").strip()
hz_deg = (circle.findtext("HorizontalCircle") or "").strip()
z_deg = (circle.findtext("VerticalCircle") or "").strip()
sd_m = (circle.findtext("EDMDistance") or "").strip()
if target_name == "" or hz_deg == "" or z_deg == "" or sd_m == "":
continue
station_seq[stid].append({
"target": target_name,
"hz_gon": deg_to_gon_str(hz_deg),
"z_gon": deg_to_gon_str(z_deg),
"sd_m": meter_str_from_jxl(sd_m),
"zh": target_height_by_id.get(target_id, ""),
})
# ========= Matching-Funktion =========
def pick_jxl_entry_for_obs(seq, start_ptr, zp, hz_csv, z_csv, sd_csv, search_window=200):
"""
Standard: nimmt seq[start_ptr]
Wenn target nicht passt: sucht im Fenster nach passendem zp.
Bei Mehrfachtreffern wird bevorzugt, wo gerundete Werte passen.
"""
if start_ptr >= len(seq):
return None, start_ptr
first = seq[start_ptr]
if first["target"] == zp:
return first, start_ptr + 1
end = min(len(seq), start_ptr + search_window)
candidates = []
for i in range(start_ptr, end):
if seq[i]["target"] == zp:
candidates.append((i, seq[i]))
if not candidates:
return first, start_ptr + 1
if len(candidates) == 1:
i, entry = candidates[0]
return entry, i + 1
good = []
for i, entry in candidates:
ok_hz = csv_is_rounding_of_jxl(hz_csv, entry["hz_gon"])
ok_z = csv_is_rounding_of_jxl(z_csv, entry["z_gon"])
ok_sd = csv_is_rounding_of_jxl(sd_csv, entry["sd_m"])
score = int(ok_hz) + int(ok_z) + int(ok_sd)
good.append((score, i, entry))
good.sort(key=lambda t: (-t[0], t[1]))
_, i_best, entry_best = good[0]
return entry_best, i_best + 1
# ========= CSV verarbeiten =========
repl_counts = {"Hz": 0, "Z": 0, "SD": 0}
current_station_id = None
current_station_ptr = 0
line_no = 0
fehlende_IH = [] # (zeilennummer, standpunkt)
fehlende_ZH = [] # (zeilennummer, standpunkt, zielpunkt)
fehlender_StationRecord = [] # (zeilennummer, standpunkt_text)
current_station_name = None
with open(CSV_IN, newline="", encoding="utf-8") as fin, open(CSV_OUT, "w", newline="", encoding="utf-8") as fout:
reader = csv.reader(fin, delimiter=";")
writer = csv.writer(fout, delimiter=";", lineterminator="\n")
for row in reader:
line_no += 1
if len(row) < 4:
row = row + [""] * (4 - len(row))
# ---- Standpunkt-Kandidat? ----
if is_station_candidate(row):
sp = row[0].strip()
# Nur als Standpunkt behandeln, wenn er wirklich in der JXL als StationName existiert:
if sp in station_names_set:
use = stationname_usecount.get(sp, 0)
recs = stationname_to_records[sp]
if use >= len(recs):
raise RuntimeError(f"Standpunkt {sp} kommt in CSV öfter vor als in der JXL (StationRecords).")
sid, ih = recs[use]
stationname_usecount[sp] = use + 1
current_station_name = sp
current_station_id = sid
current_station_ptr = 0
# fehlende IH loggen
if ih is None or str(ih).strip() == "":
fehlende_IH.append((line_no, sp))
writer.writerow([sp, f"IH:{ih}", "", "", ""])
continue
# NICHT in JXL: wenn es wie ein Standpunkt aussieht -> loggen
if sp.isdigit():
fehlender_StationRecord.append((line_no, sp))
writer.writerow(row)
continue
# ---- Beobachtung? ----
if is_obs_line(row) and current_station_id is not None:
zp = row[0].strip()
hz_csv = row[1].strip()
z_csv = row[2].strip()
sd_csv = row[3].strip()
seq = station_seq.get(current_station_id, [])
jxl_entry, new_ptr = pick_jxl_entry_for_obs(seq, current_station_ptr, zp, hz_csv, z_csv, sd_csv)
if jxl_entry is None:
writer.writerow(row)
continue
current_station_ptr = new_ptr
hz_out = hz_csv
z_out = z_csv
sd_out = sd_csv
if csv_is_rounding_of_jxl(hz_csv, jxl_entry["hz_gon"]):
hz_out = jxl_entry["hz_gon"]
repl_counts["Hz"] += 1
if csv_is_rounding_of_jxl(z_csv, jxl_entry["z_gon"]):
z_out = jxl_entry["z_gon"]
repl_counts["Z"] += 1
if csv_is_rounding_of_jxl(sd_csv, jxl_entry["sd_m"]):
sd_out = jxl_entry["sd_m"]
repl_counts["SD"] += 1
# fehlende ZH loggen
zh_val = jxl_entry.get("zh", "")
if zh_val is None or str(zh_val).strip() == "":
fehlende_ZH.append((line_no, current_station_name, zp))
last_col = f"{sd_out}:ZH:{zh_val}" if str(zh_val).strip() != "" else sd_out
writer.writerow([zp, hz_out, z_out, last_col])
continue
# ---- alles andere unverändert ----
writer.writerow(row)
print("Fertig.")
print("Ausgabe:", CSV_OUT)
print("Ersetzungen (Rundung -> JXL volle Nachkommastellen):", repl_counts)
print("\n--- Fehlende IH ---")
print("Anzahl:", len(fehlende_IH))
for z, sp in fehlende_IH[:50]:
print(f"Zeile {z}: Standpunkt {sp} (IH leer in JXL)")
if len(fehlende_IH) > 50:
print("... (weitere gekürzt)")
print("\n--- Fehlende ZH ---")
print("Anzahl:", len(fehlende_ZH))
for z, sp, zp in fehlende_ZH[:50]:
print(f"Zeile {z}: Standpunkt {sp}, Ziel {zp} (ZH nicht ermittelt)")
if len(fehlende_ZH) > 50:
print("... (weitere gekürzt)")
print("\n--- Standpunkt in CSV, aber kein StationRecord in JXL ---")
print("Anzahl:", len(fehlender_StationRecord))
for z, sp in fehlender_StationRecord[:50]:
print(f"Zeile {z}: Standpunkt {sp} (nicht in JXL als StationName gefunden)")
if len(fehlender_StationRecord) > 50:
print("... (weitere gekürzt)")

482
Vorbereitungen_Fabian/Transformation_Helmert_V3_final.py
View File

@@ -1,482 +0,0 @@
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")