Added SLAM code from Frieburg