System and method for low latency, high performance pose fusion

A method comprising:

• identifying map points in vision data captured by a camera to determine vision pose relative to a camera coordinate, wherein the captured vision data is relative to the camera coordinate;
• in response to not identifying enough of the map points in the captured vision data to determine the vision pose relative to the camera coordinate, re-initializing the vision pose to be relative to the camera coordinate using captured movement data from an inertial measurement unit (IMU) sensor, wherein the captured movement data is relative to an IMU coordinate;
• transforming the vision pose from the camera coordinate to the IMU coordinate;
• applying a scale factor, based on a combination of the vision pose and an IMU pose relative to the IMU coordinate, to the vision pose in order to generate a scaled vision pose;
• fusing the scaled vision pose and the IMU pose into a fused pose in a temporally distributed manner;
• and
• outputting the fused pose for use with a display.

A non-transitory computer readable medium embodying a computer program, the computer program comprising computer readable program code that when executed causes at least one processor to:

• identify map points in vision data captured by a camera to determine a vision pose relative to a camera coordinate, wherein the captured vision data is relative to the camera coordinate;
• in response to not identifying enough of the map points in the captured vision data to determine the vision pose relative to the camera coordinate, re-initialize the vision pose to be relative to the camera coordinate using captured movement data from an inertial measurement unit (IMU) sensor, wherein the captured movement data is relative to an IMU coordinate;
• transform the vision pose from the camera coordinate to the IMU coordinate;
• apply a scale factor, based on a combination of the vision pose and an IMU pose relative to the IMU coordinate, to the vision pose in order to generate a scaled vision pose;
• fuse the scaled vision pose and the IMU pose into a fused pose in a temporally distributed manner;
• and
• output the fused pose for use with a display.