# Mobile Robot Mapping

## Motivation for Mapping

• Insufficient GPS accuracy
• No GPS indoors
• Prerequisite for many mobile robot tasks
• Localisation
• Path planning
• Maps for humans - visualisation enables effective human robot interaction (HRI)
• Sometimes robots can be effective without it e.g. iRobot Roomba series

## Background to Mapping

• Aspects to mapping
• Localisation given a prior map
• Mapping - creating maps given the observation poses, sensor data and sensor model
• Simultaneous Localisation and Mapping SLAM - solving when both pose and map are unknown, only the sensor model and sensor data are known and perhaps and estimate of relative pose.
• Type of maps
• Geometric e.g. occupancy grids, occupied voxel lists and octree based maps. Good for both localisation and path planning.
• Topological - a graph of recognisable places and a method for following repeatable paths between these places
• Feature/landmark - a list of of recognisable features located in 2D/3D, good for localisation but not for path planning

## Conventional Notation

• Pose - position + orientation, X
• Map, M, can be
• Sensor data, Z
• Sensor model is $$p(Z|M, X)$$

Canonical form of map building, map given set of sensor measurements and corresponding poses

$$p(X_{1 \ldots t}, M | Z_{1 \ldots t})$$

Recast by Bayes' theorem

$$p(Z_{1 \ldots t} | X_{1 \ldots t}, M ) p(X_{1 \ldots t}, M) / p(Z_{1 \ldots t})$$

Simplify with Markov assumption $$p(M_t|M_{1..t-1}) = p(M_t|M_{t-1})$$ which excludes loop closure.

• Normally insufficient on its own for map building
• Good for providing high speed initial guess
• Sources of odometry
• wheel encoders
• IMU, gyroscopes
• Combination of IMU and wheel encoders can be good

## Scan matching

• Iterative Closest Point, ICP
• Cumulative errors like odometry
• Scan to scan
• Scan to map
• Address these with loop closure
• Further details see ICP section (pages 12-26) of Scan-matching

## Loop Closure

• Two main elements
• Place recognition
• Pose graph error distribution
• Loop closure dramatically improves quality of large scale maps

A Heuristic Loop Closing Technique for Large-Scale 6D SLAM by J. Sprickerhof, A. NÃ¼chter, K. Lingemann, and J. Hertzberg

## Pose graph relaxation

• Pose graph is a graph of connected poses
• connected by observed relative pose either
• from odometry
• from scan matching
• Spring network energy minimisation
• Spring energy is $$E = \frac{k_1}{2} x^2 + \frac{k_2}{2} \theta ^2$$
• For angular and translational displacements
• Energy minimised over the whole network
• Estimate position and variance of node i from each neighboring node j

## Iterative Closest Point, ICP

• Purpose to align points
• Originally devised for aligning 3D scan point clouds of objects scanned by tabletop 3D scanners
• Paper [besl1992]
• Implementations
• PCL point cloud library
• Meshlab
• MRPT Mobile robotics programming toolkit
• My python implementation

## ICP Algorithm

• For two sets of points P and Q.
• while E > threshold
• Establish correspondences e.g. by nearest neighbour
• Estimate transformation parameters using a mean square cost function.
• Transform points using the estimated parameters.
• Calculate E

## Point-to-point ICP derivation

For a rotation about the x-axis

Given the rotation matrices around the various axes.

$R_x(\theta) = \left( \begin{matrix} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \\ \end{matrix} \right), R_y(\theta) = \left( \begin{matrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \\ \end{matrix} \right), R_z(\theta) = \left( \begin{matrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0\\ 0 & 0 & 1\\ \end{matrix} \right),$

The full rotation matrix for small angles $$\alpha, \beta, \gamma$$ about the x, y, z axes is therefore

$R = \left( \begin{matrix} 1 & -\gamma & \beta \\ \gamma & 1 & -\alpha \\ -\beta & \alpha & 1 \\ \end{matrix} \right)$

The rotation matrix,

$$R =\left(\begin{matrix}1 & - \gamma & \beta\\\gamma & 1 & - \alpha\\- \beta & \alpha & 1\end{matrix}\right)$$

The translation vector

$$T =\left(\begin{matrix}t_{x}\\t_{y}\\t_{z}\end{matrix}\right)$$

The error function

$$E = R P + T - Q =\left(\begin{matrix}p_{x} + t_{x} - q_{x} + \beta p_{z} - \gamma p_{y}\\p_{y} + t_{y} - q_{y} + \gamma p_{x} - \alpha p_{z}\\p_{z} + t_{z} - q_{z} + \alpha p_{y} - \beta p_{x}\end{matrix}\right)$$

$$=\left(p_{x} + t_{x} - q_{x} + \beta p_{z} - \gamma p_{y}\right)^{2} + \left(p_{y} + t_{y} - q_{y} + \gamma p_{x} - \alpha p_{z}\right)^{2} + \left(p_{z} + t_{z} - q_{z} + \alpha p_{y} - \beta p_{x}\right)^{2}$$

To minimise E equate partial derivatives to zero.

$$\delta E / \delta \alpha = p_{y} t_{z} + p_{z} q_{y} - p_{y} q_{z} - p_{z} t_{y} - \beta p_{x} p_{y} - \gamma p_{x} p_{z} + \alpha p_{y}^{2} + \alpha p_{z}^{2} = 0$$

$$\delta E / \delta \beta = p_{x} q_{z} + p_{z} t_{x} - p_{x} t_{z} - p_{z} q_{x} - \alpha p_{x} p_{y} - \gamma p_{y} p_{z} + \beta p_{x}^{2} + \beta p_{z}^{2} = 0$$

$$\delta E / \delta \gamma = p_{x} t_{y} + p_{y} q_{x} - p_{x} q_{y} - p_{y} t_{x} - \alpha p_{x} p_{z} - \beta p_{y} p_{z} + \gamma p_{x}^{2} + \gamma p_{y}^{2} = 0$$

$$\delta E / \delta t_x = p_{x} + t_{x} - q_{x} + \beta p_{z} - \gamma p_{y} = 0$$

$$\delta E / \delta t_y = p_{y} + t_{y} - q_{y} + \gamma p_{x} - \alpha p_{z} = 0$$

$$\delta E / \delta t_z = p_{z} + t_{z} - q_{z} + \alpha p_{y} - \beta p_{x} = 0$$

Factor out the coefficients of the DOFs appropriately so it can be represented in linear form for solving.

$$A x + B = 0$$

Results in a covariance like matrix and linear matrix equation

$$\left(\begin{matrix}p_{y}^{2} + p_{z}^{2} & - p_{x} p_{y} & - p_{x} p_{z} & 0 & - p_{z} & p_{y}\\- p_{x} p_{y} & p_{x}^{2} + p_{z}^{2} & - p_{y} p_{z} & p_{z} & 0 & - p_{x}\\- p_{x} p_{z} & - p_{y} p_{z} & p_{x}^{2} + p_{y}^{2} & - p_{y} & p_{x} & 0\\0 & p_{z} & - p_{y} & 1 & 0 & 0\\- p_{z} & 0 & p_{x} & 0 & 1 & 0\\p_{y} & - p_{x} & 0 & 0 & 0 & 1\end{matrix}\right) \left(\begin{matrix}\alpha\\\beta\\\gamma\\t_{x}\\t_{y}\\t_{z}\end{matrix}\right)+\left(\begin{matrix}p_{z} q_{y} - p_{y} q_{z}\\p_{x} q_{z} - p_{z} q_{x}\\p_{y} q_{x} - p_{x} q_{y}\\p_{x} - q_{x}\\p_{y} - q_{y}\\p_{z} - q_{z}\end{matrix}\right)= 0$$

• A is symmetric so Cholesky decomposition is recommended

## Alternative ICP methods

• Derivation of point-to-plane minimization by Szymon Rusinkiewicz ICP derivation
• Generalized ICP by A. Segal, D. Haehnel and S. Thrun

## References

• Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms, Hugh Durrant-Whyte, Fellow, IEEE, and Tim Bailey
• [besl1992] A method for registration of 3D shapes - PJ Besl