Berechnung der Transformation mittels Einheitsquaternion

Zunächst wird die Berechnung der Rotation $\MR$ von der Berechnung der Translation $\mathbf{t}$ getrennt, um die Rotation separat zu bestimmen. Die Translation findet man anschließend ausgehend von der Rotation. Eine solche Entkopplung ist möglich, weil für eine Lösung $(\hat \MR, \hat \mathbf{t})$ von (3.7) gilt, dass
$M = \{\mathbf{m}_i\}_{1,\ldots,N_m}$ und $\hat D = \{\hat \MR\mathbf{d}_j - \hat \mathbf{t} \}_{1,\ldots,N_d}$ den gleichen Schwerpunkt haben.

Der erste Schritt berechnet zwei Punktmengen $M'$ und $D'$ von den Originalpunktmengen $M$ und $D$, indem von jedem Punkt der Schwerpunkt der Punktmengen subtrahiert wird. Es ist

$$\mathbf{c}_m = \frac{1}{N_m} \sum_{i=1}^{N_m} \mathbf{m}_{i},$$ (3.11)

$$\mathbf{c}_d = \frac{1}{N_d} \sum_{j=1}^{N_d} \mathbf{d}_{j}$$ (3.12)

und

$$M' = \{ \mathbf{m}'_{i} = \mathbf{m}_{i} - \mathbf{c}_{m} \}_{1,\ldots,N_m},$$ (3.13)

$$D' = \{ \mathbf{d}'_{j}\ = \mathbf{d}_{j}\, - \mathbf{c}_{d} \}_{1,\ldots,N_m}.$$ (3.14)

Die Gleichung (3.7) vereinfacht sich zu

$$E(\MR, \mathbf{t}) = \sum_{i=1}^{N_m}\sum_{j=1}^{N_d}w_{i,j}\left\lvert\left\lvert \mathbf{m}'_{i}-\MR\mathbf{d}'_j \right\rvert\right\rvert ^2.$$ (3.15)

Die Herleitung für obiges Resultat befindet sich in Anhang A.2.1. Nachdem eine Lösung für (3.15) bestimmt ist, ergibt sich die Translation als

$$\mathbf{t}= \mathbf{c}_m - \MR\mathbf{c}_d.$$

Die gesuchte Rotation $\MR$ findet man mit Hilfe der $3 \times 3$ Kovarianz-Matrix $\mathbf{M}$, die sich aus den Punkten von $M'$ und $D'$ bildet:

$$\mathbf{M}= \sum_{i=1}^{N_m}\sum_{j=1}^{N_d} w_{i,j} \ \mathbf{m}'_i {\mathbf{d}'_j}^T.$$

Diese Matrix enthält alle notwendigen Informationen, um $\MR$ unter Benutzung der Methode der kleinsten Quadrate zu berechnen. Die einzelnen Elemente von $\mathbf{M}$ bestimmen sich wie folgt:

$$\mathbf{M}= \left( \begin{array}{ccc} S_{xx} & S_{xy} & S_{xz... ...S_{yy} & S_{yz} \\ S_{zx} & S_{zy} & S_{zz} \end{array}\right)$$

mit

$$S_{xx} = \sum_{i=1}^{N_m}\sum_{j=1}^{N_d} w_{i,j} \ m'_{ix} d'_{j... ...um_{i=1}^{N_m}\sum_{j=1}^{N_d} w_{i,j} \ m'_{ix} d'_{jy}, \qquad \ldots \quad .$$

Eine symmetrische $4 \times 4$ Matrix $\mathbf{N}$ wird aus $\mathbf{M}$ berechnet:

$$\mathbf{N}= \left( \begin{array}{cccc} (S_{xx} + S_{yy} + S_{... ...x} + S_{xz}) & (- S_{xx} - S_{yy} + S_{zz}) \end{array}\right).$$ (3.16)

Die Eigenvektoren und die zugehörigen Eigenwerte von $\mathbf{N}$ müssen berechnet werden. Horn zeigt, dass das Einheitsquaternion $\dot q = (q_0, q_x, q_y, q_z)$ der gesuchten Rotation dem Eigenvektor des größten positiven Eigenwertes von $\mathbf{N}$ entspricht [40]. Der Beweis für diese Tatsache findet sich in Anhang A.2.2. Die Nullstellen des charakteristischen Polynoms der Matrix $\mathbf{N}$ sind die gesuchten Eigenwerte. Da das Polynom den Grad 4 hat, lassen sich die Eigenwerte direkt berechnen (Methode von Ferrari). Die gesuchte orthonormale Rotationsmatrix berechnet man anschließend aus dem Einheitsquaternion $\dot q$ mit (3.6).