Fix/new g2o interface