Rigide Body Transformationsmatrix

[Ede1994]

Hallo
Ich möchte eine rigide Transformation auf einen Punkt anwenden. Dafür erzeuge ich eine die notwendige Transformationsmatrix und wende diese auf den Punkt an. An sich funktioniert alles, nur stimmt das Ergebnis nicht...
Den Fehler kann ich bisher nicht ausmachen, vielleicht übersehe ich was offensichtliches!?

Code: Alles auswählen

def trig(angle):
  r = angle
  return cos(r), sin(r)

def matrix(rotation, translation):
  xC, xS = trig(rotation[0])
  yC, yS = trig(rotation[1])
  zC, zS = trig(rotation[2])
  dX = translation[0]
  dY = translation[1]
  dZ = translation[2]
  Translate_matrix = np.array([[1, 0, 0, dX],
                               [0, 1, 0, dY],
                               [0, 0, 1, dZ],
                               [0, 0, 0, 1]])
  Rotate_X_matrix = np.array([[1, 0, 0, 0],
                              [0, xC, -xS, 0],
                              [0, xS, xC, 0],
                              [0, 0, 0, 1]])
  Rotate_Y_matrix = np.array([[yC, 0, yS, 0],
                              [0, 1, 0, 0],
                              [-yS, 0, yC, 0],
                              [0, 0, 0, 1]])
  Rotate_Z_matrix = np.array([[zC, -zS, 0, 0],
                              [zS, zC, 0, 0],
                              [0, 0, 1, 0],
                              [0, 0, 0, 1]])
  print np.dot(Rotate_Z_matrix,np.dot(Rotate_Y_matrix,np.dot(Rotate_X_matrix,Translate_matrix)))
  return np.dot(Rotate_Z_matrix,np.dot(Rotate_Y_matrix,np.dot(Rotate_X_matrix,Translate_matrix)))


def transform(point, TransformArray):
  p = np.array([0,0,0,1])
  for i in range (0,len(point)-1):
      p[i] = point[i]
  p=np.dot(TransformArray,np.transpose(p))
  for i in range (0,len(point)-1):
      point[i]=p[i]
  return point
  
if __name__ == '__main__':
  point
  matrix = matrix(rotation, translation)
  print transform(point, matrix)

Bei den Winkeln handelt es sich um rad. Wenn ich die Transf.matrix printe fällt mir auf, dass die Translationswerte dX,dY,dZ nicht richtig übernommen werden. WARUM?
Hier mal ein Ergebnis:

Code: Alles auswählen

rotation = [0.0226817, -0.00242333, -0.00649134]
translation = [-0.19502307185234014, 0.9954635344879247, -9.30521311620628]
[[  9.99975995e-01   6.43466541e-03  -2.56987423e-03  -1.64699688e-01]
 [ -6.49127535e-03   9.99722075e-01  -2.26635509e-02   1.20734199e+00]
 [  2.42332763e-03   2.26796887e-02   9.99739846e-01  -9.28068813e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]]
Anfangspunkt: [123.44655, 191.39461, 16.75809 ]
Endpunkt: [124.06136880554668, 191.35583140470374, 16.75809]

Das Ergebnis sollte ca. so aussehen: 124.12617, 192.60176, 7.87802! Vor allem die z-Koord. stimmt nicht...

UPDATE:
Folgendes habe ich jetzt geändert und die Ergebnisse sehen nun besser aus:

Code: Alles auswählen

def transform(point, TransformArray):
  p = np.array([0.,0.,0.,1.])
  for i in range (0,len(point)):
      p[i] = float(point[i])
  print p
  p=np.dot(TransformArray,np.transpose(p))
  for i in range (0,len(point)):
      point[i]=p[i]
  return point

Habe ich jetzt hier noch einen groben Fehler drin?

[Sirius3]

Ich bekomme mit Deiner Tranlationsmatrix andere Werte. Ist das wirklich der Code, mit dem Du hier gerechnet hast?

Zum Code: eingerückt wird immer mit 4 Leerzeichen pro Ebene. Variabelnnamen schreibt man klein_mit_unterstrich.
Warum nennst Du `angle` in `r` um?
Direkten Indexzugriff sollte man auch bei numpy vermeiden und vor allem keine for-Schleife über einen Index. Die Funktion `transform` ändert die Werte der Übergebenen Objekte, was sie nicht tun sollte!

[Ede1994]

Ja, dass ist der Code zu den Ergebnissen...ist ja seltsam...
Ja stimmt, dass umzubenennen ist doof.... r=rad

[__deets__]

Schau dir mal slicing an. Den ganzen Unfug mit den Schleifen zur zuweisung des Vektors und das wieder rauspulen macht man nicht.

Code: Alles auswählen

p[:3] = point

Umgekehrt beim rauspulen.

Dann solltest du deine Entscheidung mit Python 2 zu arbeiten ueberdenken. Das ist ab Ende diesen Jahres nicht mehr unterstuetz.

Die Namenskonventionen fuer Python im PEP8 solltest du auch beachten, TransformArray ist fuer mich (und jeden anderen, der Python programmiert) kein Name eines Wertes, sondern ein Klassenname. transform_matrix waere der passendere Name. Oder gar nur transform. So heisst's in den meisten Bibliotheken.

Last but not least: ja, in Python faengt man ab 0 an auf Werte zuzugreifen, und range(end) liefert nur Werte bis end-1. Aber siehe oben. Macht man eh nicht.

[Ede1994]

Danke für die Tips, ich werde sich später versuchen auszubessern.

Ein Fehler muss aber noch drinstecken. Ich habe gerade mal das Vorzeichen von dZ geändert und da kommen jetzt ganz falsche Werte raus...

[Ede1994]

Ich bekomme mit Deiner Tranlationsmatrix andere Werte. Ist das wirklich der Code, mit dem Du hier gerechnet hast?

Zum Code: eingerückt wird immer mit 4 Leerzeichen pro Ebene. Variabelnnamen schreibt man klein_mit_unterstrich.
Warum nennst Du `angle` in `r` um?
Direkten Indexzugriff sollte man auch bei numpy vermeiden und vor allem keine for-Schleife über einen Index. Die Funktion `transform` ändert die Werte der Übergebenen Objekte, was sie nicht tun sollte!

Kommst du denn ungefähr auf die erwarteten Werte?
Die Funktion 'transform' dient doch der Anwendung der Matrix auf den Punkt, also müssen sich doch die Werte ändern oder habe ich dich da flasch verstanden?!

[__blackjack__]

@Ede1994: Die Funktion `transform()` sollte einen transformierten Punkt als Ergebnis liefern aber nichts an den übergebenen Objekten ändern.

[Ede1994]

@Ede1994: Die Funktion `transform()` sollte einen transformierten Punkt als Ergebnis liefern aber nichts an den übergebenen Objekten ändern.

Okay sorry, vielleicht stehe ich gerade zu sehr auf dem Schlauch, ABER tut 'transform' das nicht? Die Funktion gibt mir doch den transformierten Punkt als Ergebnis.

[__deets__]

Du hast ihn falsch verstanden. Wenn deine Funktion ein Argument bekommt, und einen Rueckgabewert liefert mit return, dann ist es HOCHGRADIG fehleranfaellig, das Argument gleichzeitig auch noch in-place zu modifizieren. Auf die Art faengt man sich die wunderbarsten Bugs ein. Ein simples "return p[:3]" ist alles, was du brauchst.

Und im Rahmen der Rundungsfehler ist der Code fuer mich auch korrekt:

Code: Alles auswählen

from math import cos, sin, radians
import numpy as np

def trig(angle):
  r = angle
  return cos(r), sin(r)

def matrix(rotation, translation):
  xC, xS = trig(rotation[0])
  yC, yS = trig(rotation[1])
  zC, zS = trig(rotation[2])
  dX = translation[0]
  dY = translation[1]
  dZ = translation[2]
  Translate_matrix = np.array([[1, 0, 0, dX],
                               [0, 1, 0, dY],
                               [0, 0, 1, dZ],
                               [0, 0, 0, 1]])
  Rotate_X_matrix = np.array([[1, 0, 0, 0],
                              [0, xC, -xS, 0],
                              [0, xS, xC, 0],
                              [0, 0, 0, 1]])
  Rotate_Y_matrix = np.array([[yC, 0, yS, 0],
                              [0, 1, 0, 0],
                              [-yS, 0, yC, 0],
                              [0, 0, 0, 1]])
  Rotate_Z_matrix = np.array([[zC, -zS, 0, 0],
                              [zS, zC, 0, 0],
                              [0, 0, 1, 0],
                              [0, 0, 0, 1]])
  return np.dot(Rotate_Z_matrix,np.dot(Rotate_Y_matrix,np.dot(Rotate_X_matrix,Translate_matrix)))


def transform(point, TransformArray):
  p = np.array([0,0,0,1])
  p[:3] = point
  p=np.dot(TransformArray,np.transpose(p))
  return p[:3]

if __name__ == '__main__':
    point = np.array([0, 0, 0])
    rotation = 0, 0, 0
    translation = 100, 10, 10
    t = matrix(rotation, translation)
    print(transform(point, t))

    point = np.array([100, 0, 0])
    rotation = 0, 0, radians(90)
    translation = 0, 0, 0
    t = matrix(rotation, translation)
    print(transform(point, t))

[__deets__]

Du hast ihn falsch verstanden. Wenn deine Funktion ein Argument bekommt, und einen Rueckgabewert liefert mit return, dann ist es HOCHGRADIG fehleranfaellig, das Argument gleichzeitig auch noch in-place zu modifizieren. Auf die Art faengt man sich die wunderbarsten Bugs ein. Ein simples "return p[:3]" ist alles, was du brauchst.

Und im Rahmen der Rundungsfehler ist der Code fuer mich auch korrekt:

Code: Alles auswählen

from math import cos, sin, radians
import numpy as np

def trig(angle):
  r = angle
  return cos(r), sin(r)

def matrix(rotation, translation):
  xC, xS = trig(rotation[0])
  yC, yS = trig(rotation[1])
  zC, zS = trig(rotation[2])
  dX = translation[0]
  dY = translation[1]
  dZ = translation[2]
  Translate_matrix = np.array([[1, 0, 0, dX],
                               [0, 1, 0, dY],
                               [0, 0, 1, dZ],
                               [0, 0, 0, 1]])
  Rotate_X_matrix = np.array([[1, 0, 0, 0],
                              [0, xC, -xS, 0],
                              [0, xS, xC, 0],
                              [0, 0, 0, 1]])
  Rotate_Y_matrix = np.array([[yC, 0, yS, 0],
                              [0, 1, 0, 0],
                              [-yS, 0, yC, 0],
                              [0, 0, 0, 1]])
  Rotate_Z_matrix = np.array([[zC, -zS, 0, 0],
                              [zS, zC, 0, 0],
                              [0, 0, 1, 0],
                              [0, 0, 0, 1]])
  return np.dot(Rotate_Z_matrix,np.dot(Rotate_Y_matrix,np.dot(Rotate_X_matrix,Translate_matrix)))


def transform(point, TransformArray):
  p = np.array([0,0,0,1])
  p[:3] = point
  p=np.dot(TransformArray,np.transpose(p))
  return p[:3]

if __name__ == '__main__':
    point = np.array([0, 0, 0])
    rotation = 0, 0, 0
    translation = 100, 10, 10
    t = matrix(rotation, translation)
    print(transform(point, t))

    point = np.array([100, 0, 0])
    rotation = 0, 0, radians(90)
    translation = 0, 0, 0
    t = matrix(rotation, translation)
    print(transform(point, t))

[Ede1994]

Ach so. Okay. Verstanden! DANKE!