Fixed compilation errors in SLAM_PAPER. Still need to do lots of cleaning up, but errors are gone