add new helper functions to util
add g2o optimization
fix optimization
extracted util interface and add util_mysql implementation