c++ Graph SLAM unfinished