I am attempting to find a way to project from world coordinates to geo coordinates with a calibrated fusion instance. To do this I am using a custom function to project from local coordinates to the geo coordinates with fusion.camera_to_geo.
This current implementation is inspired by a suggestion from @TanguyHardelin following the discovery of an issue with SDK V 4.1.1.
Data: https://drive.google.com/file/d/1XxqANF3J-y4Q6GYiCN7eC22UHCCcZ8Kr/view?usp=sharing
Code:
import pandas as pd
import pyzed.sl as sl
import json
import cv2
from datetime import datetime
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
def gnss_str_to_pyzed(data:str) -> sl.GNSSData:
"""
Converts a GNSS string to a PyZED GNSSData object.
Args:
data (str): The GNSS string to be converted.
Returns:
sl.GNSSData: The converted GNSSData object.
"""
data = json.loads(data)
gps_data = sl.GNSSData()
sl_fix_mode = sl.GNSS_MODE.FIX_2D
sl_status = sl.GNSS_STATUS.SINGLE
gps_data.gnss_mode = sl_fix_mode.value
gps_data.gnss_status = sl_status.value
date_time = datetime.fromisoformat(data["timestamp"])
zed_ts = sl.Timestamp()
zed_ts.set_nanoseconds(int(date_time.timestamp()*1e9))
gps_data.ts = zed_ts
gps_data.set_coordinates(data["lat"],data["lon"],data["elevation"],False)
covariances = json.loads(data["covariance"])
gps_data.position_covariances = [float(x) for x in covariances]
return gps_data
def xyz_to_georectified(xyz, fusion) -> np.ndarray:
"""
Converts XYZ coordinates to georectified coordinates using the given fusion object.
Args:
xyz (np.ndarray): Array of XYZ coordinates.
fusion: The fusion object used for conversion.
Returns:
np.ndarray: Array of georectified coordinates in the format [longitude, latitude, elevation].
"""
georectified_coords = []
pose = sl.Pose()
geopose = sl.GeoPose()
for point in xyz.tolist():
translation = sl.Translation()
transform = sl.Transform()
translation.init_vector(point[0], point[1], point[2])
transform.set_translation(translation)
pose.init_transform(transform)
status = fusion.camera_to_geo(pose, geopose)
lat, lon, elevation = geopose.latlng_coordinates.get_coordinates(False)
georectified_coords.append([lon, lat, elevation])
return georectified_coords
def plot_computed_vs_true_positions(positions_df:pd.DataFrame):
fig, ax = plt.subplots()
ax.plot(positions_df["lon"].to_numpy(),positions_df["lat"].to_numpy(),label="True",c='r')
ax.plot(positions_df["lon_computed"].to_numpy(),positions_df["lat_computed"].to_numpy(),label="Computed",c='b',linewidth=0.5)
ax.legend()
plt.show()
def main(input_svo_file):
zed_pose = sl.Pose()
py_translation = sl.Translation()
text_translation = ""
text_rotation = ""
init_params = sl.InitParameters(depth_mode=sl.DEPTH_MODE.ULTRA,
coordinate_units=sl.UNIT.METER,
coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP)
init_params.set_from_svo_file(input_svo_file)
# create the camera that will input the position from its odometry
zed = sl.Camera()
status = zed.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
print("[ZED][ERROR] Camera Open : " + repr(status) + ". Exit program.")
exit()
# Enable positional tracking:
positional_init = zed.enable_positional_tracking()
if positional_init != sl.ERROR_CODE.SUCCESS:
print("[ZED][ERROR] Can't start tracking of camera : " + repr(status) + ". Exit program.")
exit()
# Display
display_resolution = sl.Resolution(1280, 720)
left_img = sl.Mat()
# Create Fusion object:
fusion = sl.Fusion()
init_fusion_param = sl.InitFusionParameters()
init_fusion_param.coordinate_units = sl.UNIT.METER
init_fusion_param.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
init_fusion_param.verbose = True
fusion_init_code = fusion.init(init_fusion_param)
if fusion_init_code != sl.FUSION_ERROR_CODE.SUCCESS:
print("[ZED][ERROR] Failed to initialize fusion :" + repr(fusion_init_code) + ". Exit program")
exit()
# Enable odometry publishing:
configuration = sl.CommunicationParameters()
zed.start_publishing(configuration)
uuid = sl.CameraIdentifier(zed.get_camera_information().serial_number)
fusion.subscribe(uuid, configuration, sl.Transform(0, 0, 0))
# Enable positional tracking for Fusion object
positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
positional_tracking_fusion_parameters.enable_GNSS_fusion = True
gnss_calibration_parameters = sl.GNSSCalibrationParameters()
gnss_calibration_parameters.target_yaw_uncertainty = 7e-3
gnss_calibration_parameters.enable_translation_uncertainty_target = True
gnss_calibration_parameters.target_translation_uncertainty = 15e-2
gnss_calibration_parameters.enable_reinitialization = False
gnss_calibration_parameters.gnss_vio_reinit_threshold = 5
gnss_calibration_parameters.gnss_antenna_position = np.asarray([0.01,.01,0.01])
positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_parameters
fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
last_timestamp = sl.Timestamp()
positions = []
is_running = True
while is_running:
if zed.grab() == sl.ERROR_CODE.SUCCESS:
zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
zed.retrieve_image(left_img, sl.VIEW.LEFT, sl.MEM.CPU, display_resolution)
current_ts = zed.get_timestamp(sl.TIME_REFERENCE.IMAGE)
current_ts.set_nanoseconds(int(current_ts.get_nanoseconds() + 5e6))
cv2.imshow("left", left_img.numpy())
cv2.waitKey(10)
elif zed.grab() == sl.ERROR_CODE.END_OF_SVOFILE_REACHED:
print("End of SVO file.")
is_running = False
break
data_map = {}
ing = zed.retrieve_svo_data("GNSS",data_map,last_timestamp,current_ts)
data_map = [(k,v) for k,v in data_map.items()]
for data in data_map:
data_string = data[1].get_content_as_string()
input_gnss = gnss_str_to_pyzed(data_string)
ingest_error = fusion.ingest_gnss_data(input_gnss)
if ingest_error != sl.FUSION_ERROR_CODE.SUCCESS and ingest_error != sl.FUSION_ERROR_CODE.NO_NEW_DATA_AVAILABLE:
print("Ingest error occurred when ingesting GNSSData: ", ingest_error)
last_timestamp = current_ts
# get the fused position
if fusion.process() == sl.FUSION_ERROR_CODE.SUCCESS:
fused_position = sl.Pose()
# Get position into the ZED CAMERA coordinate system:
current_state = fusion.get_position(fused_position)
if current_state == sl.POSITIONAL_TRACKING_STATE.OK:
image_geopose = sl.GeoPose()
current_geopose_status = fusion.get_geo_pose(image_geopose)
if current_geopose_status == sl.GNSS_FUSION_STATUS.OK:
lat,lon,elev= image_geopose.latlng_coordinates.get_coordinates(False)
x,y,z = fused_position.get_translation().get().tolist()
_, yaw_std, position_std = fusion.get_current_gnss_calibration_std()
if yaw_std != -1 and yaw_std<1:
gnss_vio_matrix = fusion.get_geo_tracking_calibration().m
xyz_np = np.array([x,y,z,1]).T
xyz_np = np.dot(gnss_vio_matrix,xyz_np)
x,y,z = xyz_np[:3]
position_data = {
"lat":lat,
"lon":lon,
"elev":elev,
"x":x,
"y":y,
"z":z,
}
print(position_data,flush=True)
positions.append(
position_data
)
positions_df = pd.DataFrame(positions)
positions_df_xyz = positions_df[["x","y","z"]].to_numpy()
positions_df_georectified = xyz_to_georectified(positions_df_xyz,fusion)
# add computed georectified coordinates to the dataframe
positions_df["lon_computed"] = [x[0] for x in positions_df_georectified]
positions_df["lat_computed"] = [x[1] for x in positions_df_georectified]
positions_df["elev_computed"] = [x[2] for x in positions_df_georectified]
positions_df.to_csv("positions.csv")
fusion.close()
zed.close()
return positions_df
if __name__ == "__main__":
input_svo_file = "camera_recording_0.svo2"
positions_df: pd.DataFrame = main(input_svo_file)
plot_computed_vs_true_positions(positions_df)
From the image below, you can see that the current approach does not work.
