Add triangulation to calculate the end of the Pen
Write to file top camera parameters