Changed main2.cpp to SLAM.cpp