Fixed bug in bundle adjust, still sucks though