Calibration with vision marker works

This is the first release.
The code lacks documentation and cleaning but the code works!