\heading{Motion Stereo Mathematics}
Here we go over the ground covered briefly in the
{\bf 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 = \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
\mattt{AA}{AB}{AC}{BA}{BB}{BC}{CA}{CB}{CC}\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\null+(AA^2+AB^2+AC^2)\; \overline{X_2^2}\cr
\qquad\null+(BA^2+BB^2+BC^2)\; \overline{Y_2^2}\cr
\qquad\null+(CA^2+CB^2+CC^2)\; \overline{Z_2^2}\cr
\qquad\null-2\;\biglp\quad
AA\; \overline{X_1\; X_2} + AB\; \overline{Y_1\; X_2} + AC\; \overline{Z_1\; X_2}\quad\quad\cr
\qquad\null+BA\; \overline{X_1\; Y_2} + BB\; \overline{Y_1\; Y_2} + BC\; \overline{Z_1\; Y_2}\cr
\qquad\null+CA\; \overline{X_1\; Z_2} + CB\; \overline{Y_1\; Z_2} + CC\; \overline{Z_1\; Z_2}\cr
\qquad\null-(AA\; BA + AB\; BB + AC\; BC)\; \overline{X_2\; Y_2}\cr
\qquad\null-(AA\; CA + AB\; CB + AC\; CC)\; \overline{X_2\; Z_2}\cr
\qquad\null-(BA\; CA + BB\; CB + BC\; CC)\; \overline{Y_2\; Z_2}\cr
\qquad\null+(\overline{X_1}-AA\; \overline{X_2}-BA\; \overline{Y_2}-CA\; \overline{Z_2})\; X_c\cr
\qquad\null+(\overline{Y_1}-AB\; \overline{X_2}-BB\; \overline{Y_2}-CB\; \overline{Z_2})\; Y_c\cr
\qquad\null+(\overline{Z_1}-AC\; \overline{X_2}-BC\; \overline{Y_2}-CC\; \overline{Z_2})\; Z_c
\quad\bigrp\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}\cr
\qquad\null+(AA^2+AB^2+AC^2)\; \overline{X_2^2}\cr
\qquad\null+(BA^2+BB^2+BC^2)\; \overline{Y_2^2}\cr
\qquad\null+(CA^2+CB^2+CC^2)\; \overline{Z_2^2}\cr
\qquad\null-2\;\biglp\quad
AA\; \overline{X_1\; X_2} + AB\; \overline{Y_1\; X_2} + AC\; \overline{Z_1\; X_2}\cr
\qquad\null+BA\; \overline{X_1\; Y_2} + BB\; \overline{Y_1\; Y_2} + BC\; \overline{Z_1\; Y_2}\cr
\qquad\null+CA\; \overline{X_1\; Z_2} + CB\; \overline{Y_1\; Z_2} + CC\; \overline{Z_1\; Z_2}\cr
\qquad\null-(AA\; BA + AB\; BB + AC\; BC)\; \overline{X_2\; Y_2}\cr
\qquad\null-(AA\; CA + AB\; CB + AC\; CC)\; \overline{X_2\; Z_2}\cr
\qquad\null-(BA\; CA + BB\; CB + BC\; CC)\; \overline{Y_2\; Z_2}\quad\bigrp\cr
\qquad\null-(\overline{X_1}-AA\; \overline{X_2}-BA\; \overline{Y_2}-CA\; \overline{Z_2})^2\cr
\qquad\null-(\overline{Y_1}-AB\; \overline{X_2}-BB\; \overline{Y_2}-CB\; \overline{Z_2})^2\cr
\qquad\null-(\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
$$\mattt{-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}$$
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.
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 ]\;
{ \mattt{\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}}^{-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 {\sl Macsyma} symbolic
mathematics system \ref{M1}.