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:

Center image point, so we can assume pinhole model

With a
C = (x,y,z)
calcC/z
,C/z
isC'
(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) 
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.

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 byz
(in theory, the code recalculates, and I haven't refactored because of float/double differences in the result) 
We apply the reverse of
C = RW + T
to get the point in world coordinates
Closes #118 (closed)