Hi all,
I’m using a Jetson Xavier NX with JetPack 5.0.2 and ZED SDK 5.0.6 to record data. I also use a u-blox 8 USB device to record GNSS data, which is recorded to the SVO2 file as well - I followed the samples given for global localization.
In a post-processing step (done on another PC with more memory) I want to use positional tracking to track the camera. For that, I use the fusion module, enable positional tracking, and ingest the GNSS data. However, when getting POSITIONAL_TRACKING_FUSION_STATUS, it always returns VISUAL_INERTIAL and does not seem to integrate the GNSS values (for which it should return VISUAL_INERTIAL_GNSS according to the documentation).
The MWE below shows my configuration. Is there anything I need to enable to use GNSS in the fused positional tracking?
import pyzed.sl as sl
import numpy as np
import json
# The following class is taken from the official ZED SDK examples.
class GNSSReplay:
def __init__(self, zed):
self._zed = zed
self.current_gnss_idx = 0
self.previous_ts = 0
self.last_cam_ts = 0
self.gnss_data = None
self.initialize()
def initialize(self):
keys = self._zed.get_svo_data_keys()
gnss_key = "GNSS_json"
if gnss_key not in keys:
print("SVO doesn't contain GNSS data")
exit(1)
else:
ts_begin = sl.Timestamp()
data = {}
self.gnss_data = {"GNSS": []}
err = self._zed.retrieve_svo_data(gnss_key, data, ts_begin, ts_begin)
for k, d in data.items():
gnss_data_point_json = json.loads(d.get_content_as_string())
gnss_data_point_formatted = {}
if "Geopoint" in gnss_data_point_json:
latitude = gnss_data_point_json["Geopoint"]["Latitude"]
longitude = gnss_data_point_json["Geopoint"]["Longitude"]
altitude = gnss_data_point_json["Geopoint"]["Altitude"]
gnss_data_point_formatted["ts"] = gnss_data_point_json[
"EpochTimeStamp"
]
latitude_std = gnss_data_point_json["Eph"]
longitude_std = gnss_data_point_json["Eph"]
altitude_std = gnss_data_point_json["Epv"]
else:
latitude = gnss_data_point_json["coordinates"]["latitude"]
longitude = gnss_data_point_json["coordinates"]["longitude"]
altitude = gnss_data_point_json["coordinates"]["altitude"]
gnss_data_point_formatted["ts"] = gnss_data_point_json["ts"]
latitude_std = gnss_data_point_json["latitude_std"]
longitude_std = gnss_data_point_json["longitude_std"]
altitude_std = gnss_data_point_json["altitude_std"]
gnss_data_point_formatted["coordinates"] = {
"latitude": latitude,
"longitude": longitude,
"altitude": altitude,
}
gnss_data_point_formatted["latitude_std"] = latitude_std
gnss_data_point_formatted["longitude_std"] = longitude_std
gnss_data_point_formatted["altitude_std"] = altitude_std
gnss_data_point_formatted["position_covariance"] = {
longitude_std + longitude_std,
0,
0,
0,
latitude_std + latitude_std,
0,
0,
0,
altitude_std + altitude_std,
}
if "mode" in gnss_data_point_json:
gnss_data_point_formatted["mode"] = gnss_data_point_json["mode"]
if "status" in gnss_data_point_json:
gnss_data_point_formatted["status"] = gnss_data_point_json["status"]
if "fix" in gnss_data_point_json:
gnss_data_point_formatted["fix"] = gnss_data_point_json["fix"]
gnss_data_point_formatted["original_gnss_data"] = gnss_data_point_json
self.gnss_data["GNSS"].append(gnss_data_point_formatted)
def is_microseconds(self, timestamp):
return 1_000_000_000_000_000 <= timestamp < 10_000_000_000_000_000
def is_nanoseconds(self, timestamp):
return 1_000_000_000_000_000_000 <= timestamp < 10_000_000_000_000_000_000
def getGNSSData(self, gnss_data, gnss_idx):
current_gnss_data = sl.GNSSData()
if gnss_idx >= len(gnss_data["GNSS"]):
print("Reached the end of the GNSS playback data.")
return current_gnss_data
current_gnss_data_json = gnss_data["GNSS"][gnss_idx]
if (
current_gnss_data_json["coordinates"] is None
or current_gnss_data_json["coordinates"]["latitude"] is None
or current_gnss_data_json["coordinates"]["longitude"] is None
or current_gnss_data_json["coordinates"]["altitude"] is None
or current_gnss_data_json["ts"] is None
):
print("Null GNSS playback data.")
return current_gnss_data_json
gnss_timestamp = current_gnss_data_json["ts"]
ts = sl.Timestamp()
if self.is_microseconds(gnss_timestamp):
ts.set_microseconds(gnss_timestamp)
elif self.is_nanoseconds(gnss_timestamp):
ts.set_nanoseconds(gnss_timestamp)
else:
print("Warning: Invalid timestamp format from GNSS file")
current_gnss_data.ts = ts
current_gnss_data.set_coordinates(
current_gnss_data_json["coordinates"]["latitude"],
current_gnss_data_json["coordinates"]["longitude"],
current_gnss_data_json["coordinates"]["altitude"],
False,
)
current_gnss_data.longitude_std = current_gnss_data_json["longitude_std"]
current_gnss_data.latitude_std = current_gnss_data_json["latitude_std"]
current_gnss_data.altitude_std = current_gnss_data_json["altitude_std"]
position_covariance = [
current_gnss_data.longitude_std**2,
0.0,
0.0,
0.0,
current_gnss_data.latitude_std**2,
0.0,
0.0,
0.0,
current_gnss_data.altitude_std**2,
]
current_gnss_data.position_covariances = position_covariance
if "mode" in current_gnss_data_json:
current_gnss_data.gnss_mode = current_gnss_data_json["mode"]
if "status" in current_gnss_data_json:
current_gnss_data.gnss_status = current_gnss_data_json["status"]
if "fix" in current_gnss_data_json:
gpsd_mode = current_gnss_data_json["fix"]["mode"]
sl_mode = sl.GNSS_MODE.UNKNOWN
if gpsd_mode == 0:
sl_mode = sl.GNSS_MODE.UNKNOWN
elif gpsd_mode == 1:
sl_mode = sl.GNSS_MODE.NO_FIX
elif gpsd_mode == 2:
sl_mode = sl.GNSS_MODE.FIX_2D
elif gpsd_mode == 3:
sl_mode = sl.GNSS_MODE.FIX_3D
gpsd_status = current_gnss_data_json["fix"]["status"]
sl_status = sl.GNSS_STATUS.UNKNOWN
if gpsd_status == 0:
sl_status = sl.GNSS_STATUS.UNKNOWN
elif gpsd_status == 1:
sl_status = sl.GNSS_STATUS.SINGLE
elif gpsd_status == 2:
sl_status = sl.GNSS_STATUS.DGNSS
elif gpsd_status == 3:
sl_status = sl.GNSS_STATUS.RTK_FIX
elif gpsd_status == 4:
sl_status = sl.GNSS_STATUS.RTK_FLOAT
elif gpsd_status == 5:
sl_status = sl.GNSS_STATUS.SINGLE
elif gpsd_status == 6:
sl_status = sl.GNSS_STATUS.DGNSS
elif gpsd_status == 7:
sl_status = sl.GNSS_STATUS.UNKNOWN
elif gpsd_status == 8:
sl_status = sl.GNSS_STATUS.UNKNOWN
elif gpsd_status == 9:
sl_status = sl.GNSS_STATUS.SINGLE
current_gnss_data.gnss_mode = sl_mode.value
current_gnss_data.gnss_status = sl_status.value
return current_gnss_data
def getNextGNSSValue(self, current_timestamp):
current_gnss_data = self.getGNSSData(self.gnss_data, self.current_gnss_idx)
if current_gnss_data is None or current_gnss_data.ts.data_ns == 0:
return current_gnss_data
if current_gnss_data.ts.data_ns > current_timestamp:
current_gnss_data.ts.data_ns = 0
return current_gnss_data
last_data = current_gnss_data
step = 1
while True:
last_data = current_gnss_data
diff_last = current_timestamp - current_gnss_data.ts.data_ns
current_gnss_data = self.getGNSSData(
self.gnss_data, self.current_gnss_idx + step
)
if current_gnss_data is None or current_gnss_data.ts.data_ns == 0:
break
if current_gnss_data.ts.data_ns > current_timestamp:
if current_gnss_data.ts.data_ns - current_timestamp > diff_last:
current_gnss_data = last_data
break
self.current_gnss_idx += 1
return current_gnss_data
def grab(self, current_timestamp):
current_data = sl.GNSSData()
current_data.ts.data_ns = 0
if current_timestamp > 0 and current_timestamp > self.last_cam_ts:
current_data = self.getNextGNSSValue(current_timestamp)
if current_data.ts.data_ns == self.previous_ts:
current_data.ts.data_ns = 0
self.last_cam_ts = current_timestamp
if current_data.ts.data_ns == 0:
return sl.ERROR_CODE.FAILURE, None
self.previous_ts = current_data.ts.data_ns
return sl.ERROR_CODE.SUCCESS, current_data
### Main function
def main():
# Hardcoded configuration
init_params = sl.InitParameters(
depth_mode=sl.DEPTH_MODE.NEURAL,
coordinate_units=sl.UNIT.METER,
coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP,
svo_real_time_mode=False,
)
# PLEASE PROVIDE A VALID SVO FILE
init_params.set_from_svo_file(
"<path-to-svo2-file>"
)
zed = sl.Camera()
status = zed.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
print(f"Camera Open: {status}. Exit program.")
exit(1)
# Enable positional tracking
pose_tracking_params = sl.PositionalTrackingParameters()
pose_tracking_params.mode = sl.POSITIONAL_TRACKING_MODE.GEN_3
pose_tracking_params.enable_area_memory = False
positional_init = zed.enable_positional_tracking(pose_tracking_params)
if positional_init != sl.ERROR_CODE.SUCCESS:
zed.close()
print(f"Can't start tracking of camera: {positional_init}. Exit program.")
exit(1)
# Enable communication
communication_params = sl.CommunicationParameters()
communication_params.set_for_shared_memory()
zed.start_publishing(communication_params)
# Enable fusion
fusion_params = sl.InitFusionParameters()
fusion_params.coordinate_units = init_params.coordinate_units
fusion_params.coordinate_system = init_params.coordinate_system
fusion_params.verbose = True
fusion = sl.Fusion()
fusion_init_code = fusion.init(fusion_params)
if fusion_init_code != sl.FUSION_ERROR_CODE.SUCCESS:
print(f"Failed to initialize fusion: {fusion_init_code}. Exit program")
exit(1)
camera_info = zed.get_camera_information()
uuid = sl.CameraIdentifier(camera_info.serial_number)
fusion.subscribe(uuid, communication_params, sl.Transform(0, 0, 0))
gnss_calibration_parameters = sl.GNSSCalibrationParameters()
gnss_calibration_parameters.target_yaw_uncertainty = 0.1
gnss_calibration_parameters.enable_translation_uncertainty_target = False
gnss_calibration_parameters.target_translation_uncertainty = 0.1
gnss_calibration_parameters.enable_reinitialization = True
gnss_calibration_parameters.gnss_vio_reinit_threshold = 5
gnss_calibration_parameters.enable_rolling_calibration = True
gnss_calibration_parameters.gnss_antenna_position = np.array([0.06, 0.002, 0.0])
positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
positional_tracking_fusion_parameters.enable_GNSS_fusion = True
positional_tracking_fusion_parameters.gnss_calibration_parameters = (
gnss_calibration_parameters
)
fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
# Setup GNSS replay
gnss_replay = GNSSReplay(zed)
rt_params = sl.RuntimeParameters()
img_pose = sl.Pose()
while True:
camera_status = zed.grab(rt_params)
if camera_status == sl.ERROR_CODE.SUCCESS:
zed.get_position(img_pose, sl.REFERENCE_FRAME.WORLD)
gnss_status, gnss_data = gnss_replay.grab(
img_pose.timestamp.get_nanoseconds()
)
if gnss_status == sl.ERROR_CODE.SUCCESS:
ingest_error = fusion.ingest_gnss_data(gnss_data)
if (
ingest_error != sl.FUSION_ERROR_CODE.SUCCESS
and ingest_error != sl.FUSION_ERROR_CODE.NO_NEW_DATA_AVAILABLE
):
print(f"Ingest error: {ingest_error}")
fusion_process_error = fusion.process()
if fusion_process_error != sl.FUSION_ERROR_CODE.SUCCESS:
print(f"Fusion process error: {fusion_process_error}")
(geopose_status, yaw_std, position_std) = (
fusion.get_current_gnss_calibration_std()
)
fused_status = fusion.get_fused_positional_tracking_status()
print(
f"GNSS Calibr.: {geopose_status} | GNSS Calibr. Yaw Std: {yaw_std:.3f} | GNSS Calibr. Position Std: [{position_std[0]:.3f}, {position_std[1]:.3f}, {position_std[2]:.3f}] | Fused Pos. Tr. Status: {fused_status.gnss_fusion_status} | {fused_status.tracking_fusion_status}"
)
elif camera_status == sl.ERROR_CODE.END_OF_SVOFILE_REACHED:
print("End of SVO file reached.")
break
else:
continue
zed.close()
fusion.close()
if __name__ == "__main__":
main()
GNSS Calibration and Fused Positional Tracking both return OK when running the script. Here is an exemplary output
...
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: CALIBRATION_IN_PROGRESS | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: CALIBRATION_IN_PROGRESS | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: CALIBRATION_IN_PROGRESS | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: CALIBRATION_IN_PROGRESS | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: CALIBRATION_IN_PROGRESS | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: CALIBRATION_IN_PROGRESS | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: OK | VISUAL INERTIAL
GNSS Calibr.: OK | GNSS Calibr. Yaw Std: 0.098 | GNSS Calibr. Position Std: [0.491, 0.733, 0.544] | Fused Pos. Tr. Status: OK | VISUAL INERTIAL
...
Thanks for your help!