Added the g2o library and implementation