SFM: Added P3P algorithm and test