Continued implementing SLaM. Fixed compile errors.