Heuristic 2

This implements heuristic 2 of the Lie Group Solver

TODO:

- [x] Add more tests