Method and Apparatus for Wide Area Multi-Body 6D Pose Tracking System

A method for tracking a position and orientation of one or more objects in three-dimensional space, comprising:

• receiving orientation measurements for each object acquired from an inertial measurement unit of a tracked sensor unit attached that object;
• acquiring, from each of a plurality of position sensitive detectors, a respective 2D location measurement of at least one light source of the tracked sensor unit attached to each object in response to light from the at least one light source being focused onto the plurality of position sensitive detectors by respective optical lenses;
• triangulating a 3D position of each object from the respective 2D location measurements acquired from the plurality of position sensitive detectors;
• and
• calculating a six-dimensional pose of each object including 3D position and 3D orientation of the object based on the triangulated 3D position and the received orientation measurements.

An apparatus for tracking position and orientation of an ultrasound probe in 3D space, comprising:

• a tracked sensor unit attached to the ultrasound probe, the tracked sensor unit comprising a plurality of light emitting diodes arranged in a pattern around an axis of the ultrasound probe and an inertial measurement unit;
• and
• a position sensitive detector tracking device comprising:
• a plurality of position sensitive detector sensors combined with optical lenses that focus light emitted from one or more of the plurality of light emitting diodes of the tracked sensor unit onto each position sensitive detector sensor, and
• a processing unit configured to calculate the position and orientation of the ultrasound probe in three-dimensional space from output of the inertial measurement unit of the tracked sensor unit attached to the ultrasound probe and output of the plurality of position sensitive detector sensors in response to light emitted from at least one of the plurality of light emitting diodes of the tracked sensor unit being focused on the each position sensitive detector sensor by the optical lenses.