Abstract

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 .