SFM: Unfinished code to test homography