Update slam.cpp