Feature/initial commit of g2o wrapper

Catkinized. :)

bad, but ok if necessary