Bug fixes and optimizations for sample consensus prerejective (replaces #736)

This PR solves a few bugs, and makes a number of significant improvements on both speed and robustness of the **sample_consensus_prerejective** class. It is simply a rehash of #736 with a clean up of the commits.