I am using the PositionalTracking module to get the camera pose in the world frame following a script similar to example code tutorial. I am having an issue with the translation accuracy, which seems to drift quickly and can have very large errors (sometimes around 30cm). I am just running the program, lifting the camera up and down a few times (with smooth movements), and setting it back down on the table so that the height should return to 0.
System Information
Camera: Zed Mini
SDK Version: 5.0.2
OS: Ubuntu 24
Python version: 3.10
Code
import sys
import time
import numpy as np
import pyzed.sl as sl
zed = sl.Camera()
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD720
init_params.camera_fps = 30
init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
init_params.coordinate_units = sl.UNIT.METER
zed.open(init_params)
tracking_params = sl.PositionalTrackingParameters()
tracking_params.enable_imu_fusion = True
tracking_params.mode = sl.POSITIONAL_TRACKING_MODE.GEN_1
status = zed.enable_positional_tracking(tracking_params) #enable Positional Tracking
if status != sl.ERROR_CODE.SUCCESS:
print("[Sample] Enable Positional Tracking : "+repr(status)+". Exit program.")
zed.close()
sys.exit()
runtime_parameters = sl.RuntimeParameters()
zed_pose = sl.Pose()
translation = sl.Translation()
orientation = sl.Orientation()
try:
while True:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Get the pose of the camera relative to the world frame
state = zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
Tx = np.round(zed_pose.get_translation(translation).get()[:3], 3)
print(f"Translation: {Tx}, timestamp: {zed_pose.timestamp.get_seconds()}")
time.sleep(0.001)
finally:
zed.close()
Output
I am looking at the value of the Y axis, which is the height. It should return back to 0 since I lift the camera up starting on a tabletop and then place it down on the same table. As you can see below there is 17cm of error. In other repetitions of this experiment I sometimes see even larger error.
Translation: [0.0, 0.0, 0.0]
...
...
...
Translation: [-0.13 -0.174 -0.028]
Other Information: I tested with GEN_2 and GEN_3 as well. GEN_3 seems to provide much more accurate results in the same experiment (less than 1cm error). GEN_2 still has a lot of error.
Question: Is this level of accuracy expected or am I doing something wrong? Is there anything else I can do to improve the accuracy besides what is shown in my code?