Refactor of 5pt solver with added detection of degeneracy

Change-Id: I5b41fe0ce884b349df391fe432f09217cfd4e988