For many augmented reality guidance, teleoperation, or human-robot
interaction systems, accurate, fast, and robust 6 degree of freedom
object pose tracking is essential. However, current solutions easily
lose tracking when line-of-sight to markers is lost. In this paper we
present a tracking system which matches or improves on current methods
in speed and accuracy, achieving 1.77 mm and 1.51 degrees accuracy at 22
Hz. Reflective markers are segmented in infrared images using contour
detection before using the known marker geometry to perform point
correspondence and pose computation using novel approaches. At the same
time, a new square root unscented Kalman filter is introduced which
improves accuracy and flexibility by tracking the markers themselves
rather than the computed pose, and enables fusion of an external
inertial measurement unit. This reduces noise and makes the tracking
robust to brief loss of line-of-sight. The algorithms and methods are
described in detail with pseudo-code for ease of reproduction. The
system is implemented in simulation and on a Microsoft HoloLens 2 using
Unity for ease of entry and integration into graphical projects. The
code is made available open source. Tests of the system are described,
and the results analyzed .