Refactoring of get3DPoint

This MR refactors get3DPoint to be more easily understood.

get3DPoint works with the pinhole camera model. We also assume that the extrinsic calibration has calculated the rotation R and translation T of the camera, such that

C = R  W + T

is valid for every C in camera coordinates and the according W in world cordinates.

The calculation in get3DPoint is essentially:

1. Center image point, so we can assume pinhole model

2. With a C = (x,y,z) calc C/z, C/z is C' (This is the same as the pinhole projection of our point with assumed depth 1, since all 3D points projecting to the given pixel are on a line)

3. Get the depth (z) of the object by solving the third row of

\frac{C}{z} = C' = \frac{RW + T}{z}

which means

c_3' = \frac{h+t}{z} \Leftrightarrow z = \frac{h+t}{c_3'}

where h denotes the x_3 value in the world coordinate system, aka the height.

4. With the known depth, use the pinhole model to get the correct point. It is one the same line as the projection C' with depth 1, so we just multiply by z (in theory, the code recalculates, and I haven't refactored because of float/double differences in the result)

5. We apply the reverse of C = RW + T to get the point in world coordinates

Closes #118 (closed)