Feature/initial commit of g2o wrapper

Catkinized. :)