Added distance mesurement in CalibCam.
Updated test main function.