More code cleanup and updated comments

Update README.md
Merge branch 'master' of github.com:nvaligi/wip-gtsam

Conflicts:
	07_VO_motel/vicon.cpp