Updated to work with g2o update on openslam.org
Updated source code to fix g2o break