Get bundle adjustment working, quite slow