Added Monte Carlo driver for 3D SLAM with known features