Rewrite part of the five-point algorithm code

Hi, 

My previous 5-point algorithm contribution exploits some code online, which I noticed may have potential license issues. This pull request rewrote that part.