Implemented more slam. 
the app crashes in SlamManager.cpp (i think there is a problem in the particles removal)