First working version for scale
Scaling still a bit unreliable due to Rotation matrices with negative values for diagonals
Merge branch 'master' of github.com:aukejw/AI-Project--Visual-SLAM

Conflicts:
	src/naostream.cpp