WIP - Heuristic 2

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