\documentstyle[12pt]{article}
\begin{document}
\section{Introduction}
This document provides information on the various coordinate systems used
in the experiment, the measurements taken and the methods used to find
coordinate transformations. Actual values of the parameters are also
provided.

The experiments were all performed using Martin Marietta's Autonomous Land 
Vehicle (ALV). The ALV is equipped with cameras, digitizers and computers.
For our experiment we used a color camera (Sony  ) and a Vicom digitizer.
Some important parameters of the camera and digitizer are provided in
the file camera\_info.dat .

The ALV also has an inertial platform - or Land Navigation System (LNS) on
board. The LNS consists of 3 gyroscopes and an odometer (see \cite{} for more
details). 

Some of this information is taken from a Martin-Marietta draft document.

\section{Notation}
A point will be denoted by its position vector {\bf p} = $(x,y,z)^T$. We will
use lowercase letters in boldface to denote vectors and uppercase letters in
boldface to denote matrices. 

An angle will be denoted by specifying an axis (e.g. X and the rotation
$\alpha$ about it. eg (X,$\alpha$) . The right-hand rule is used to determine the sign of the
angle. Thus for example the positive angle is the direction in which
a right-hand screw turns when pointing in the direction of the axis. 

\subsection{Homogeneous Coordinates}
A position vector {\bf p} is represented in homogeneous coordinates as
{\bf p} = $(x,y,z,1)^T$. 

A translation ${\bf T}_{vw}$ which takes a vector ${\bf p}_v$ in the v
coordinate system to a vector ${\bf p}_w$ in the w coordinate system is
specified by the homogeneous matrix
\begin{equation}
{\bf T}_{vw} = \begin{array}{cccc}
1 & 0 & 0 & x \\
0 & 1 & 0 & y \\
0 & 0 & 1 & z \\
0 & 0 & 0 & 1
\end{array}
\end{equation}
In matrix notation this is
\begin{equation}
{\bf p}_w = {\bf T}_{vw}{\bf p}_v
\end{equation}

Or equivalently
\begin{equation}
{\bf p}_w = {\bf p}_v + (x,y,z)^T
\end{equation}

Homogeneous rotation matrices are represented by the axis and angle
about which the rotation occurs. Thus for example a rotation $(X,\alpha)$
is represented as: 
\begin{equation}
{\bf R}_{X,\alpha} = \begin{array}{cccc}
1 & 0 & 0 & 0 \\
0 & cos \alpha & -sin \alpha & 0 \\
0 & sin \alpha & cos \alpha & 0 \\
0 & 0 & 0 & 1
\end{array}
\end{equation}
a rotation $(Y,\beta)$ as:
\begin{equation}
{\bf R}_{Y,\beta} = \begin{array}{cccc}
cos \beta & 0 & sin \beta & 0 \\
0 & 1 & 0 & 0 \\
-sin \beta & 0 & cos \beta & 0 \\
0 & 0 & 0 & 1
\end{array}
\end{equation}
and a rotation $(Z,\gamma)$ as:
\begin{equation}
{\bf R}_{Z,\gamma} = \begin{array}{cccc}
cos \gamma & -sin \gamma & 0 & 0 \\
sin \gamma & cos \gamma & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{array}
\end{equation}

An arbitrary 3-d rotation {\bf R} could be represented by
a rotation $(X,\alpha)$ followed by a rotation $(Y,\beta)$ followed by
another rotation $(Z,\gamma)$ 
\begin{equation}
{\bf R} = {\bf R}_{Z,\gamma}{\bf R}_{Y,\beta}{\bf R}_{X,\alpha}
\end{equation}

A homogeneous rotation ${\bf R}_{vw}$ would thus map a vector ${\bf p}_v$ in
a coordinate system v into the vector ${\bf p}_w$ in the rotated coordinate
system w
\begin{equation}
{\bf p}_w = {\bf R}_{vw}{\bf p}_v
\end{equation}

\section{Coordinate Systems}
There are four coordinate systems to consider.
\begin{enumerate}
\item A world coordinate system defined by ($x_w,y_w,z_w$).
 The y-axis points North, the x-axis points East and the z-axis is parallel
to the direction of gravity (\cite{}).
\item A vehicle coordinate system ($x_v,y_v,z_v$). The x-axis points in the
direction of forward travel of the vehicle, the y-axis points to the left
of the vehicle and the z-axis points upwards. The LNS provides measurements
of the vehicle's coordinate system with respect to the world coordinate
system in the following manner \cite{}:
\begin{enumerate}
\item It gives the translation ${\bf t}_{vw} = (x_w,y_w,z_w)$  of the origin of
the vehicle coordinate system with respect to the world coordinate system.
\item It provides the vehicle azimuth angle $\psi$ - i.e the angle from the
world y-axis to the vehicle x-axis.
\item The vehicle pitch angle $\gamma$ is the magnitude of the rotation
about the y-axis of the vehicle i.e. $(Y_v,\gamma)$
\item The roll of the vehicle $\eta$ is given by the magnitude of the angle
about the x-axis of the vehicle. i.e. $(X_v,\eta)$
\end{enumerate}
\item A theodolite coordinate system $(x_t,y_t,z_t)$. The origin is defined
to be at theodolite-M and the negative x-axis in the direction of theodolite-D.
The
y-axis is in the same horizontal plane as the x-axis (horizontal with respect
to gravity). The z-axis is parallel to the direction of gravity and points
upward. (Note that
the z-axes of the world and theodolite coordinate systems point in the
same direction - thus any coordinate conversion between them essentially
reduces to a 2-D problem).
\item A camera coordinate system $(x_c,y_c,z_c)$. The origin is defined to
be at the optic centre of the camera. The z-axis is perpendicular to the
camera with its positive direction given by the right-hand rule.
\end{enumerate}

\section{Coordinate Transformations}
All coordinate transformations will be expressed in terms of homogeneous
matrices. This allows us to specify the translation and rotation using
just one matrix eg. The vehicle to world coordinate transformation is given
by:
\begin{equation}
{\bf p}_{w} = {\bf H}_{vw} {\bf p}_{v}
\end{equation}
where the coordinates of a point p are given in the  vehicle coordinate system
by ${\bf p}_{v}$ and in the world coordinate system by ${\bf p}_{w}$.
${\bf H}_{vw}$ is the
homogeneous transformation which takes the vector ${\bf p}_v$ to ${\bf p}_w$.

{\bf Notation}
In homogeneous coordinates the vector ${\bf p}_v = (x_v,y_v,z_v,1)$.
(We will use ${\bf r}_v = (x_v,y_v,z_v)$ to indicate the corresponding 3-D
position vector). The same convention will be followed for all coordinate
systems.

\subsection{Camera to Vehicle Transformation}
\begin{equation}
{\bf p}_{c} = {\bf H}_{cv} {\bf p}_{c}
\end{equation}
where
\begin{enumerate}
\item ${\bf p}_c$ is the position vector of p in camera coordinates.
\item ${\bf p}_v$ is the position vector of p in vehicle coordinates.
\item ${\bf H}_{cv}$ is the homogeneous matrix which takes ${\bf p}_c$ to
${\bf p}_v$. 

Since the camera was rigidly fixed with respect to the vehicle, this
transformation remains the same across all the sequences. 
The values of the matrix are provided in the file cam\_to\_veh\_homo.dat .

\subsection{Vehicle to World Transformation}
\begin{equation}
{\bf p}_{w} = {\bf H}_{vw} {\bf p}_{v} 
\end{equation}
where
\begin{enumerate}
\item ${\bf p}_w$ is the position vector of p in world coordinates.
\item ${\bf p}_v$ is the position vector of p in vehicle coordinates.
\item ${\bf H}_{vw}$ is the homogeneous matrix which takes ${\bf p}_v$ to
${\bf p}_w$
\end{enumerate}

The values of ${\bf H}_{vw}$ are provided in the appropriate directory
in the file veh\_to\_world\_homo.dat .


\subsection{World to Theodolite Coordinates}
Let us assume that we have the 3-d coordinates of n points in world
and theodolite coordinates. Then we need to solve the 2-D absolute
orientation problem which maps a point $(x_w,y_w)$ to $(x_t,y_t)$
(note that the world and theodolite coordinate systems have their z-axes
parallel to each other). 
	Although the z-axes are parallel their origins are shifted with
respect to each other. This is also solved for to find the transformation.

\begin{equation}
{\bf p}_{t} = {\bf H}_{wt} {\bf p}_{w} 
\end{equation}
where
\begin{enumerate}
\item ${\bf p}_t$ is the position vector of p in theodolite coordinates.
\item ${\bf p}_w$ is the position vector of p in world coordinates.
\item ${\bf H}_{wt}$ is the homogeneous matrix thich takes ${\bf p}_w$ to
${\bf p}_t$
\end{enumerate}

The values of this homogeneous matrix are provided in the appropriate
directories in the file world\_to\_theo\_homo.dat

\end{document}
