Fix sim problems

after much deliberation
Lightly simulate solvePnP