Da war ich gestern nicht ganz auf der Spur.

Um die Verschiebung und die Rotation zusammenzufassen bedient man sich der homogenen Transformation. Die bietet auch elegant die Möglichkeit auch mehrere Operationen zusammenzufassen.

Ich nehme mal diese Folien.

http://www.informatik.hs-mannheim.de/~ihme/robotik_2004ws/05_Transformationen.pdf

Fangen wir mit Folie 32 zweiter Punkt an. Dort findet sich die Gleichung welche die Koordinatenwerte zwischen zwei unterschiedlichen Koordinatensystemen transformiert.

T(A,B) eine 4X4 Matrix, besteht aus der Rotationsmatrix R und dem Verschiebungsvektor T und einer 4 ten Zeile mit 0,0,0,1

Die Bedeutung der letzten Zeile von T(A,B) findet sich auf Folie 29.

Um vom Koordinatensystem K1 (x1,y1,z1) zum Koordinatensystem K2 (x2,y2,z2)
zu kommen, must du zuerst ins Koordinatensystem K0 (x0,y0,z0) und von da aus ins Koordinatensystem K2.

Das heißt du brauchst eine Matrix T(K1->K0) und eine weitere T(K0->K2). Diese lassen
sich mit den vorhandenen Daten und den Folien 20-22 erstellen.

Wenn du direkt von K1 nach K2 möchtest, multiplizerst du die beiden Matritzen und erhältst die die Matrix T(K1->K2) = T(K0->K2) * T(K1->K0) für die Gleichung auf
Folie 32.

Auf Folie 33 ist unten gezeigt wie die multiplizierte Matrix zusammengesetzt ist.

Hoffentlich hilft das weiter.

Wenn du mehrere Scanns nacheinander machst und diese zusammenführen möchtest, würde ich die Daten nur nach K0 transformieren und dort alle weiteren Berechnungen machen. Anschließend nur die benötigeten Daten ins aktuelle Koordinatensystem Kx
transformieren. Sonst rechnet man sich zum Schluss zu Tode.