add SLAM code