Split up OldActualSlam into AbstractSLAMProblem and GraphSLAM. Renamed some stuff, (imo) cleaned some stuff up.. hope you still like it taghi :P

Seems to work fine, with one exception: I changed things so that it is no longer necessary for the robot to have seen every single landmark in a simulation (I saw in your code that if the robot missed a landmark, you'd just start everything over again). When that happens now though, it still crashes. Did not delete OldActualSlam yet so it can be used for comparison