Added some g2o types and made it all running a lot faster by removing the old jacobian code