Significant changes: Changed how the position of a map is calculated to make...

Significant changes: Changed how the position of a map is calculated to make first stage of loop detection work better. Changed optimisation hessian matrix to not use loop closure constraints and changed optimiser to only optimise loop closure constraints. Changed information matrix scaling for loop closing constraints. Fixed a bug where an array in optimiser was being used uninitialized