@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!