Got it all working with g2o, need better data