In this equation N
is the number of points,
is the mean of the points, and the known
3D marker positions and the centroids received from a camera module are used to con- struct the points pn q). F is a function of q, a quaternion rotation, i.e., a quaternion with unit length. The construction of pn q) is achieved by constructing a line for each marker that extends from the 3D position of the marker in a direction that depends on the corre-
sponding 2D centroid coordinates. cloud.
The intersection points of these lines define the point
The method to construct the lines for intersection is as follows. First, consider the case where the orientation of a camera’s coordinate frame matches the world coordinate frame. For example, the camera looks along the Z world coordinate axis such that its X and Y axes match the X and Y axes of the world coordinate frame. Next, for each marker centroid that the module is following, create a vector that points towards the focal point of the camera using the 2D coordinates of the centroid and the focal length of the camera. To explain this further let a centroid have coordinates given by c = [x, y] in pixels. Then the created vector
v = [ xk,
in metres where k is the length of a pixel in metres. Construct lines using the marker’s 3D location and the direction given by these vectors. The lines will intersect at a single point at the camera’s focal point. This intersection point is the camera’s position in world coordinates. This is illustrated in Figure 7.6. For convenience of illustration this is shown in 2D. The camera is shown as the blue circle with the centroids shown as red circles. The intersecting lines are shown in green dashes and vectors in blue.
The points pn are created by finding the intersection point between each line and every
other line. If there are M markers then there are M lines. This gives N =M
C 2 c o m b i n a -
tions of lines combined two ways and the same number of intersection points. The quater- nion q in the Equation 7.1 describes the rotation between the world coordinate frame and the camera coordinate frame and is used to rotate each vector. Using the vector v from Equation 7.2, a rotated vector v0 is created using the quaternion product
q v = q q v q ∗ ,
where qv is a quaternion with vector part equal to v and qv is the quaternion whose vector
part is equal to the rotated vector v0. For this example, q = (1, 0, 0, 0) because the world frame has the same orientation as the camera frame.
Now consider the following scenario. The camera’s coordinate system is a rotation and translation of the world coordinate system. The rotation is unknown but it can be ex- pressed by a quaternion. Following the same steps as above, first calculate vectors from