Continued implementing SLaM.

Note this version of slam.cpp is compiling with errors. Fix these first.