Lighthouse Tracker Calibration

This documentation describes the algorithm to determine the pose of a Lighthouse Base Station from the Lighthouse Tracker.

The implementation of this algorithm uses the Passer Linear Algebra library to computer things like rotations and angles. [Linear Algebra documentation]

The code for this algorithm is part of the Lighthouse Tracker repository. The actual file containing this file is BasestationPoseEstimator.cpp.

Base-station orientation

Each base-station periodically (around every 10 seconds) sends a gravity vector. This is the starting point to determine the base-station orientation because with that, we only need to determine the yaw (the rotation around the vertical axis) of the base-station.

Pitch & roll

To get the pitch and roll of the base-station, we transform the gravity vector to a Quaternion orientation:

Quaternion baseStationPitchRoll =
    Quaternion::FromToRotation(gravity, Vector3::down)

Yaw

The yaw will be determined relative to the forward direction of the lighthouse tracker. So we need to determine this forward direction relative to the base-station. We will use the Direction of the rays from the base-station to each sensor on the lighthouse tracker to determine orientation.

As the yaw is a rotation in the horizontal plane, we can determine the forward direction of the lighthouse tracker by looking at the points where the rays from the back and front rays intersect a horizontal plane. The direction from the intersection of the rays to the back sensors on the lighthouse deck to the intersection of the rays to the front sensors gives the forward direction. The length of this direction does not have any value: we only need the direction to be able to rotate the base-station correctly.

So we have to calculate the intersection point for each ray with a horizontal plane. We choose to start the rays at 1 meter high here (which is an arbitrary choice) and calculate where they cross the plane at a height of 0 meters.

Vector3 baseStationOrigin = Vector3::up; // 1 meter above origin
Vector3 intersections[4];
for (int sensorIx = 0; sensorIx < 4; sensorIx++) {
    Vector3 ray = baseStationPitchRoll * rawRays[sensorIx];
     intersections[sensorIx] = GetIntersectionPoint(baseStationOrigin, ray);
}

To improve the accuracy we are going to use both the direction of the two sensors on the left and right sides of the lighthouse tracker to determine the forward direction:

Vector3 leftDirection = intersections[2] - intersections[0];
Vector3 rightDirection = intersections[3] - intersections[1];
Vector3 fwdDirection = leftDirection + rightDirection;

(As the length of the vectors have no meaning, we do not need to devide the fwdDireciton by 2 here.)

Now we can calculate the yaw rotation:

float baseStationYaw = 
    Vector3::SignedAngle(fwdDirection, Vector3::forward, Vector3::up);

Orientation

And together with the pitch-roll this will give the base-station orientation:

Quaternion baseStationOrientation =
    Quaternion::AngleAxis(baseStationYaw, Vector3::up) * baseStationPitchRoll;

Base-station position

Now that we have the orientation of the base-stations, we can start calculating the their position relative to the lighthouse tracker.

Frist we need to recalculate the rays and intersection points now that we know the full orientation of the base-station:

Vector3 rays[4];
Vector3 intersections[4];
for (int sensorIx = 0; sensorIx < 4; sensorIx++) {
    rays[sensorIx] = baseStationOrientation * rawRays[sensorIx];
    intersections[sensorIx] =
        GetIntersectionPoint(baseStationOrigin, rays[sensorIx];
}

Distance on the ground floor

Now comes the hard part: we are going to use some trigonometry to calculate the length of the rays. We do that by first calculate the length of the ray projected on the ground floor.
In this document I will illustratie this with the rays to sensor 0 (back-left) and 2 (front-left), but in the final code I actually calculate the distance to the middle points between the sensors on the left and right for improved accuracy. I think it is a bit easier to understand if we use the sensor rays here.

First we project the rays on the ground floor. This is actually the vector from the origin (0,0,0) to the intersection point, which can be simplified to the position of the intersection point:

Vector3 projectedDirection0 = intersections[0];
        // actually intersections[0] - Vector3::zero
Vector3 projectedDirection2 = intersections[2];

We can calculate the angle betwen the projected rays and the angle of the projected ray of sensor 2 relative to the forward direction:

float projectedAngle02 =
      Vector3::Angle(projectedDirection0, projectedDirection2);
float projectedANgle2 = Vector3::Angle(Vector3::forward, projectedDirection2);

As we know that the distrance between sensors 0 and 2 is 3 cm, we can use the law of sines to calculate the distance from the origin to the intersection point of sensor0:

alpha / sin (a) = beta / sin(b)

multiply both sides with sin(b):

beta = alpha / sin(a) * sin(b)

Now, we can use the law of sines to calculate the distance:

projectedDistance0 = 0.03 / sin (projectedAngle02) * sin(projectedAngle2)

float a = 0.03f; // Distance between sensors 0 and 3
float alpha = projectedAngle02;
float beta = projectedAngle2;
float sinAlpha = sinf(alpha * Angle::Deg2Rad);
float sinBeta = sinf(beta * Angle::Deg2Rad);
float projectedDistance0 = a / sinAlpha * sinBeta;

Ray length in 3D

We are almost there, because now we know the distance to sensor 0 projected on the floor, and we know the pitch angle of the ray to sensor0, we can use the sine function to compute the length of the ray itself:

float pitchRay0 = Vector3::Angle(Vector3::down, rays[0]);
float rayLength = projectedDistance0 / sinf(pitchRay0 * Angle::Deg2Rad);

Position

Now we can compute the position of the basestation by following the ray back from the lighthouse tracker sensor using the found length. We will use the center of the lighthouse tracker as the origin. As sensor 0 is 7.5mm to the left and 15 mm to the back relative to the center of the tracker, we will use that point as the start of our computation:

Vector3 baseStationPosition =
    Vector3(-0.0075f, 0.0f, -0.015f) - 
    ray[0].normalized() * rayLength;

Now we found the position and orientation of the base-station. We only have to do this for each base-station to get all data we need to track the lighthouse tracker from now on.