Fixed homography and recover_position

Able to move on to the new stuff!