Using PyOP2 to solve graph-based SLAM