Here we go over the ground covered briefly in the
**Motion Stereo** section of Chapter 7 in gory detail.

We want to find the three dimensional rotation and translation that, if applied to the co-ordinates of the features at the first position, minimizes the weighted sum of the squares of the distances between the transformed first co-ordinates and the raw co-ordinates of the corresponding points at the second position.

The least squares expression to be minimized is $$Error \quad = \quad \sum_{i=1}^N{(X_{1,i}-X_{2,i}^\prime )^2 + (Y_{1,i}-Y_{2,i}^\prime )^2 + (Z_{1,i}-Z_{2,i}^\prime )^2 \over U_i^2}$$ where the $X_{1,i}$, $Y_{1,i}$ and $Z_{1,i}$ are the co-ordinates of the features as observed from the first cart position. $X_{2,i}$, $Y_{2,i}$ and $Z_{2,i}$ are the co-ordinates of the points as seen from the second position. $X_{2,i}^\prime $, $Y_{2,i}^\prime $ and $Z_{2,i}^\prime $ are the points from the second position subjected to a co-ordinate transformation in the attempt to make them match the first set. $U_i$ is the combined uncertainty in the position of feature $i$ in both frames. To get the most out of our data, we need to weight features proportional to their accuracy. To simplify the subsequent formulas, the $U_i$'s are normalized to make the sum of their inverse squares unity.

For a general linear transformation, these values are defined by $$[X_{2,i}^\prime , Y_{2,i}^\prime , Z_{2,i}^\prime ] \quad = \quad [X_{2,i}, Y_{2,i}, Z_{2,i}] \quad \begin{bmatrix}{AA}&{AB}&{AC}\\{BA}&{BB}&{BC}\\{CA}&{CB}&{CC}\end{bmatrix}\quad + \quad [X_c, Y_c, Z_c]$$ where $AA$ through $CC$ is the “rotation” part of the transform and $X_c$ through $Z_c$ is the translation.

$Error$ can be expanded out and the summation can be distributed over the terms, giving us $$\eqalign{Error \quad = \quad X_c^2+Y_c^2+Z_c^2+\overline{X_1^2}+\overline{Y_1^2}+\overline{Z_1^2}\cr \qquad\;+(AA^2+AB^2+AC^2)\; \overline{X_2^2}\cr \qquad\;+(BA^2+BB^2+BC^2)\; \overline{Y_2^2}\cr \qquad\;+(CA^2+CB^2+CC^2)\; \overline{Z_2^2}\cr -2\;\bigg( \; AA\; \overline{X_1\; X_2} + AB\; \overline{Y_1\; X_2} + AC\; \overline{Z_1\; X_2}\quad\quad\cr \qquad\;+BA\; \overline{X_1\; Y_2} + BB\; \overline{Y_1\; Y_2} + BC\; \overline{Z_1\; Y_2}\cr \qquad\;+CA\; \overline{X_1\; Z_2} + CB\; \overline{Y_1\; Z_2} + CC\; \overline{Z_1\; Z_2}\cr \qquad\;-(AA\; BA + AB\; BB + AC\; BC)\; \overline{X_2\; Y_2}\cr \qquad\;-(AA\; CA + AB\; CB + AC\; CC)\; \overline{X_2\; Z_2}\cr \qquad\;-(BA\; CA + BB\; CB + BC\; CC)\; \overline{Y_2\; Z_2}\cr \qquad\;+(\overline{X_1}-AA\; \overline{X_2}-BA\; \overline{Y_2}-CA\; \overline{Z_2})\; X_c\cr \qquad\;+(\overline{Y_1}-AB\; \overline{X_2}-BB\; \overline{Y_2}-CB\; \overline{Z_2})\; Y_c\cr \qquad\;+(\overline{Z_1}-AC\; \overline{X_2}-BC\; \overline{Y_2}-CC\; \overline{Z_2})\; Z_c \;\bigg) \cr}$$ where $\overline{X_1}$ is $\sum_i{X_{1,i} \over U_i^2}$ and $\overline{X_1\; Z_2}$ is $\sum_i{X_{1,i}Z_{2,i} \over U_i^2}$ and similarly for the other barred quantities. With the expression in this form, the barred quantities can be evaluated once and for all at the start of the solving.

$Error$ can be differentiated partially with respect to $X_c$, $Y_c$ and $Z_c$. If the resulting linear expressions are equated to zero and solved, we obtain

$$X_c \quad = \quad \overline{X_1} - AA\; \overline{X_2} - BA\; \overline{Y_2} - CA\; \overline{Z_2}$$ $$Y_c \quad = \quad \overline{Y_1} - AB\; \overline{X_2} - BB\; \overline{Y_2} - CB\; \overline{Z_2}$$ $$Z_c \quad = \quad \overline{Z_1} - AC\; \overline{X_2} - BC\; \overline{Y_2} - CC\; \overline{Z_2}$$

These are the components of the translation vector that minimizes $Error$ for a general “rotation” matrix. They can be back substituted into $Error$, making it a function of the rotation alone. $$\eqalign{Error\quad = \quad \overline{X_1^2}+\overline{Y_1^2}+\overline{Z_1^2}\qquad\qquad\qquad\qquad\cr \qquad\;+(AA^2+AB^2+AC^2)\; \overline{X_2^2}\cr \qquad\;+(BA^2+BB^2+BC^2)\; \overline{Y_2^2}\cr \qquad\;+(CA^2+CB^2+CC^2)\; \overline{Z_2^2}\cr -2\;\bigg( \; AA\; \overline{X_1\; X_2} + AB\; \overline{Y_1\; X_2} + AC\; \overline{Z_1\; X_2}\cr \qquad\;+BA\; \overline{X_1\; Y_2} + BB\; \overline{Y_1\; Y_2} + BC\; \overline{Z_1\; Y_2}\cr \qquad\;+CA\; \overline{X_1\; Z_2} + CB\; \overline{Y_1\; Z_2} + CC\; \overline{Z_1\; Z_2}\cr \qquad\;-(AA\; BA + AB\; BB + AC\; BC)\; \overline{X_2\; Y_2}\cr \qquad\;-(AA\; CA + AB\; CB + AC\; CC)\; \overline{X_2\; Z_2}\cr \qquad\;-(BA\; CA + BB\; CB + BC\; CC)\; \overline{Y_2\; Z_2}\;\bigg) \cr \qquad\quad-(\overline{X_1}-AA\; \overline{X_2}-BA\; \overline{Y_2}-CA\; \overline{Z_2})^2\cr \qquad\quad-(\overline{Y_1}-AB\; \overline{X_2}-BB\; \overline{Y_2}-CB\; \overline{Z_2})^2\cr \qquad\quad-(\overline{Z_1}-AC\; \overline{X_2}-BC\; \overline{Y_2}-CC\; \overline{Z_2})^2\cr}$$

The linear first approximation is now found by partially differentiating this new $Error$ with respect to $AA$ through $CC$, setting these derivatives to zero and simultaneously solving the system of nine equations (which happen to be linear) so obtained for $AA$ through $CC$.

Now we get to the hard part. The matrix obtained in the preceding step is a general 3x3 transform, and not necessarily a true rotation. A true 3D rotation has only three free parameters, not nine. However these three parameters are chosen, some or all of the elements of a rotation matrix that characterizes the same rotation will be non-linear functions of the three parameters.

We have chosen to represent rotations by a three element subset of a quaternion. A quaternion is a four element parameterization of a rotation, but any of the four elements may be derived from the other three (the sum of their squares is unity).

Imagine an arbitrary axis through the origin, described by its angles $\alpha$, $\beta$ and $\gamma$ with the co-ordinate axes. A general rotation can be specified as a twist of an angle $\theta$ around such an axis. The quaternion for this rotation is the four element list $$[ P_0, P_x, P_y, P_z] \quad = \quad [\, \cos{\theta / 2}\, , \, \cos{\alpha}\,\sin{\theta / 2} \, , \, \cos{\beta}\,\sin{\theta / 2}\, , \, \cos{\gamma}\,\sin{\theta / 2} \, ]$$ The half angle components may seem a little peculiar, but the manipulations are simplified because of them. The rotation matrix corresponding to this quaternion is $$\begin{bmatrix}{-1 + 2 P_0^2 + 2 P_x^2}&{2 P_x P_y - 2 P_0 P_z}&{2 P_0 P_y + 2 P_x P_z }\\{2 P_x P_y + 2 P_0 P_z}&{-1 + 2 P_0^2 + 2 P_y^2}&{-2 P_0 P_x + 2 P_y P_z }\\{-2 P_0 P_y + 2 P_x P_z}&{2 P_0 P_x + 2 P_y P_z}&{-1 + 2 P_0^2 + 2 P_z^2}\end{bmatrix}$$

The $P_0$ element can be defined positive and equal to $\sqrt{1-P_x^2-P_y^2-P_z^2}$ With this last substitution, the rotation is defined by three independent variables.

Substituting these matrix elements (now involving square roots for $P_0$) in place of $AA$ through $CC$ in $Error$ creates an expression only a barbarian would inflict upon a reader.

But it's now the distant future!

The web in 2011 demands one inflict too much information on readers!

CRASH.SAI is the main Cart navigation program.

CRASHS.SUB contains the bulk of the code used by CRASH.SAI, including the full $Error$ expression.

Are we barbarians yet?

Suffice it to say that the resulting expression is partially differentiated with respect to $P_x$, $P_y$ and $P_z$, and the resulting, very non-linear, expressions are equated to zero. Let $E$ represent the barbarous $Error$ parameterized by the three quaternion sine elements. Name the three expressions as follows $$E_x = {\partial E \over \partial P_x}\; , \quad E_y = {\partial E \over \partial P_y}\; , \quad E_z = {\partial E \over \partial P_z}$$

The Newton's method iteration is defined by $$[ P_x , P_y , P_z ]^\prime \quad = \quad [ P_x , P_y , P_z ]\; - \; [ E_x , E_y , E_z ]\; {\begin{bmatrix} {\partial E_x \over \partial P_x}&{\partial E_x \over \partial P_y }&{\partial E_x \over \partial P_z}\\{\partial E_y \over \partial P_x }&{\partial E_y \over \partial P_y}&{\partial E_y \over \partial P_z }\\{\partial E_z \over \partial P_x}&{\partial E_z \over \partial P_y }&{\partial E_z \over \partial P_z}\end{bmatrix}}^{-1}$$

The derivatives in the matrix, as well as $E_x$ through $E_z$ are messy closed form expressions of $P_x$, $P_y$ and $P_z$.

The bulk of the code in this part of the program, about a page of
solid formulas, was produced automatically by MIT's *Macsyma* symbolic
mathematics system \ref{M1}.