Position calculation in detail
NOTE: All calculations are implemented in geometry.cpp
This is the simplest part of the calculation. Given the time offset between sync pulse and laser plane sweep pulse in microseconds, we need to determine the angle of that sweep plane. Taking into account that the plane makes half rotation (180 deg or PI rad) each cycle (8333 µs), we can easily get the formula:
angle = (<time offset in µs> - <central time offset = 4000>) * PI / <cycle period = 8333>
Let's start with the coordinate system of one base station. It's a bit unusual, but it's what the Vive gives us. Station itself is centered in the origin, Z axis is pointing in the opposite direction of where the station is "looking", Y is up, X is left:
Now, horizontal laser plane rotates around the X axis, while vertical rotates around Y axis. Here is the horizontal plane:
A 3d plane can be described by a point on it (origin in our case) and a normal vector V. As horizontal plane rotates around X axis, its normal vector V lies on YZ plane and is rotated with the same rotation angle as the plane. Given sin/cos rotation formulas, we get the following XYZ coordinates of normals:
V_hor = (0, cos(angle_hor), sin(angle_hor))
V_ver = (cos(angle_ver), 0, -sin(angle_ver))
NOTE: When you look in the same direction as the base station, the horizontal swipe goes top-to-bottom, while vertical swipe goes left-to-right.
NOTE: Technically, the laser sweep planes are offset from the center of base station by ~3cm, but we will ignore it for now.
3d line can be described by a point on it (we can use origin) and a vector U along the line. To get the intersection line, we can use the fact that this line should lie on both planes, which means U must be perpendicular to both V_hor and V_ver. Therefore, we can calculate U as a cross product of V_hor and V_ver:
U = V_hor × V_ver
NOTE: While not required, it's recommended to normalize U after this operation to minimize calculation errors.
Now that we know the 3d lines in each of the stations' reference frames, we need to convert it to a single, global reference frame to be able to intersect them. Conversion of line vectors is easy - just multiply vectors by the station rotation matrices (M1, M2):
U1_glob = M1 * U1_loc
U2_glob = M2 * U2_loc
Conversion of line origin points is even easier - we just use station centers.
NOTE: Station rotation matrices and centers are currently hardcoded as lightsources constant in the code. See issue #2.
Now that we know 2 lines, time to 'intersect' them. We'll use a general algorithm described in e.g. http://geomalgorithms.com/a07-_distance.html#Distance-between-Lines to calculate points on these lines that are closest to the other line and use a point in middle as the result.
Line equations: P(s) = P0 + s*U and Q(t) = Q0 + t*V
Where s, t are scalar parameters, P0, Q0 - line origin points (in our case station centers), U, V - line vectors.
W0 = P0 - Q0 (difference between line origins)
a = U * U (dot products)
b = U * V
c = V * V
d = U * W0
e = V * W0
denom = a * c - b * b (denominator must be > 0 for non-parallel lines)
s = (b * e - c * d) / denom (parameter of closest point on first line to the second line)
t = (a * e - b * d) / denom (same for second line)
P(s) and Q(t) are the closest points on corresponding lines to the other line.
Distance between those points gives us the quality of solution. It shouldn't be more than a couple cm.
Result: (P(s) + Q(t)) / 2 (middle point)
In the previous step, we got the resulting point in Vive coordinate system. It's the same as used inside Vive VR, uses meters as units and has the origin in the center of the room on the floor. If you need to convert it to other coordinate system like NED, this is the time to do it, again with matrix multiplication.
NOTE: For NED, you'll need to determine the angle between North and Vive's X axis.