Rescaling map after init to make sure its same as wiggle scale
Seems like there are a few bugs in the pose estimation, last commit before I start fixing stuff