Item 1: Odometry Extrapolation.

We first extrapolate the odometry readings to all six degrees of freedom using previous registration matrices. The robot pose is the 6-vector $\V {P} = (x, y, z, \theta_{x}, \theta_{y}, \theta_{z})$ or, equivalently the tuple containing the rotation matrix and translation vector, written as 4$\times$4 OpenGL-style matrix $\M P$ [7].¹The change of the robot pose $\Delta \M P$ given the odometry information $(x_n,z_n,\theta_{y,n})$, $(x_{n+1},z_{n+1},\theta_{y,n+1})$ and the registration matrix $\M R({\theta_{x,{n}}}, {\theta_{y,{n}}}, {\theta_{z,{n}}})$ is calculated by solving:

$$\left( \begin{array}{c} x_{n+1} \\ y_{n+1} \\ z_{n+1} \\ {... ...Delta {\theta_{z,{n+1}}} \\ \end{array}\right).}_{\Delta \V P}$$

Therefore, calculating $\Delta \V P$ requires a matrix inversion. Finally, the 6D pose $\M P_{n+1}$ is calculated by

$$\M P_{n+1} = \Delta \M P \cdot \M P_{n}$$

using the poses' matrix representations.