Got pose estimation working perfectly. Cleaned up code a bit
Everything running from one folder