I am attempting to perform SLAM with VIO/GNSS Fusion. After a a few minutes, my positional uncertainty as given by fusion.get_current_gnss_calibration_std() becomes quite large (> 1e19) and my calibration becomes off. Could this be some oversight on my end with GNSS ingestion?
Note that uncertainties will be printed to the terminal
Data:
https://drive.google.com/file/d/1XxqANF3J-y4Q6GYiCN7eC22UHCCcZ8Kr/view?usp=sharing
Code:
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
########################################################################
"""
This sample shows how to fuse the position of the ZED camera with an external GNSS Sensor
"""
import sys
import pyzed.sl as sl
import json
from datetime import datetime
import cv2
import numpy as np
from typing import Tuple
import matplotlib.pyplot as plt
def gnss_str_to_pyzed(data:str) -> sl.GNSSData:
# '{"timestamp":"2024-05-06T20:13:09+00:00",
# "time_delta":0,
# "lat":46.8655633,"lon":-113.975665,
# "elevation":985.933,
# "covariance":"[34.727449, 0.0, 0.0, 0.0, 47.651408999999994, 0.0, 0.0, 0.0, 1.940449]"}'
#
data = json.loads(data)
gps_data = sl.GNSSData()
sl_fix_mode = sl.GNSS_MODE.FIX_3D
sl_status = sl.GNSS_STATUS.RTK_FIX
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
if __name__ == "__main__":
gnss_measurements = []
fused_position_measurements = []
fused_geopose_measurements = []
cv2.namedWindow("RGB",cv2.WINDOW_NORMAL)
filepath = "camera_recording_0.svo2"
input_type = sl.InputType()
input_type.set_from_svo_file(filepath) #Set init parameter to run from the .svo
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False,camera_resolution=sl.RESOLUTION.AUTO,
coordinate_units=sl.UNIT.METER,
coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP)
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE
zed = sl.Camera()
status = zed.open(init)
if status != sl.ERROR_CODE.SUCCESS: #Ensure the camera opened succesfully
print("Camera Open", status, "Exit program.")
exit(1)
print('External SVOData channels:', zed.get_svo_data_keys())
# some variables
odometry_pose = sl.Pose()
odometry_pose = sl.Pose()
py_translation = sl.Translation()
pose_data = sl.Transform()
rgb = sl.Mat()
text_translation = ""
text_rotation = ""
current_gnss = sl.GNSSData()
current_geopose = sl.GeoPose()
# Create a ZED camera object
# set up communication parameters and start publishing
communication_parameters = sl.CommunicationParameters()
communication_parameters.set_for_shared_memory()
zed.start_publishing(communication_parameters)
# warmup for camera
if zed.grab() != sl.ERROR_CODE.SUCCESS:
print("Camera grab: " + repr(status) + ". Exit program.")
exit()
else:
zed.get_position(odometry_pose, sl.REFERENCE_FRAME.WORLD)
tracking_params = sl.PositionalTrackingParameters()
# These parameters are mandatory to initialize the transformation between GNSS and ZED reference frames.
tracking_params.enable_imu_fusion = True
tracking_params.set_gravity_as_origin = True
err = zed.enable_positional_tracking(tracking_params)
if (err != sl.ERROR_CODE.SUCCESS):
print("Camera positional tracking: " + repr(status) + ". Exit program.")
exit()
camera_info = zed.get_camera_information()
# step 2
# init the fusion module that will input both the camera and the GPS
fusion = sl.Fusion()
init_fusion_parameters = sl.InitFusionParameters()
init_fusion_parameters.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
init_fusion_parameters.coordinate_units = sl.UNIT.METER
gnss_calibration_params = sl.GNSSCalibrationParameters()
gnss_calibration_params.gnss_vio_reinit_threshold = 5
fusion.init(init_fusion_parameters)
positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
positional_tracking_fusion_parameters.enable_GNSS_fusion = True
positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_params
fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
print(gnss_calibration_params)
uuid = sl.CameraIdentifier(camera_info.serial_number)
print("Subscribing to", uuid.serial_number, communication_parameters.comm_type) #Subscribe fusion to camera
status = fusion.subscribe(uuid, communication_parameters, sl.Transform(0,0,0))
if status != sl.FUSION_ERROR_CODE.SUCCESS:
print("Failed to subscribe to", uuid.serial_number, status)
exit(1)
x = 0
i = 0
if zed.grab() == sl.ERROR_CODE.SUCCESS:
zed.get_position(odometry_pose, sl.REFERENCE_FRAME.WORLD)
zed.retrieve_image(rgb, sl.VIEW.LEFT)
rgb_img = cv2.cvtColor(rgb.get_data(), cv2.COLOR_RGBA2RGB)
last_timestamp = rgb.timestamp
cv2.imshow("RGB",rgb_img)
while i < 1e7:
# get the odometry information
if zed.grab() == sl.ERROR_CODE.SUCCESS:
zed.get_position(odometry_pose, sl.REFERENCE_FRAME.WORLD)
zed.retrieve_image(rgb, sl.VIEW.LEFT)
rgb_img = cv2.cvtColor(rgb.get_data(), cv2.COLOR_RGBA2RGB)
cv2.imshow("RGB",rgb_img)
elif zed.grab() == sl.ERROR_CODE.END_OF_SVOFILE_REACHED:
break
ts_camera : sl.Timestamp = rgb.timestamp
data_map = {}
ing = zed.retrieve_svo_data("GNSS",data_map,last_timestamp,ts_camera)
data_map = [(k,v) for k,v in data_map.items()]
for data in data_map:
data_string = data[1].get_content_as_string()
gnss_data = gnss_str_to_pyzed(data_string)
time_delta = ts_camera.get_microseconds() - gnss_data.ts.get_microseconds()
status = fusion.ingest_gnss_data(gnss_data)
if status.name == "SUCCESS":
gnss_measurements.append(
[gnss_data.get_coordinates(False)]
)
last_timestamp = ts_camera
# get the fused position
if fusion.process() == sl.FUSION_ERROR_CODE.SUCCESS:
fused_tracking_state = fusion.get_position(odometry_pose, sl.REFERENCE_FRAME.WORLD)
if fused_tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
rotation = odometry_pose.get_rotation_vector()
translation = odometry_pose.get_translation(py_translation)
fused_position_measurements.append(translation.get())
# get geopose
gp_status = fusion.get_geo_pose(current_geopose)
if gp_status.name == "OK":
fused_geopose_measurements.append(
current_geopose.latlng_coordinates.get_coordinates(False)
)
process_metrics: Tuple[sl.FUSION_ERROR_CODE,sl.FusionMetrics] = fusion.get_process_metrics()
fusion_error_code, fusion_metrics = process_metrics
fusion_metrics = {"mean_camera_fused":np.round(fusion_metrics.mean_camera_fused,3),"mean_stdev_camera_ts":np.round(fusion_metrics.mean_stdev_between_camera,3)}
vio_gnss_trns: sl.Transform = fusion.get_geo_tracking_calibration()
vio_gnss_trns_translation:np.ndarray = vio_gnss_trns.get_translation().get()
gnss_calibration_data: Tuple[sl.GNSS_FUSION_STATUS,float,np.ndarray] = fusion.get_current_gnss_calibration_std()
gnss_status, yaw_unc, pos_unc = gnss_calibration_data
out = {
"fusion_error_code": fusion_error_code.name,
"fusion_metrics": fusion_metrics,
"vio_gnss_trns": [np.round(x,3) for x in vio_gnss_trns_translation.tolist()],
"gnss_calibration":gnss_status.name,
"yaw_unc": np.round(yaw_unc,3),
"pos_unc": [np.round(x,3) for x in pos_unc.tolist()]
}
print(out)
i = i + 1
if cv2.waitKey(1) & 0xFF == ord('q'):
break
zed.close()
# Assuming gnss_measurements and fused_geopose_measurements are lists of [lat, lon] pairs
gnss_lats, gnss_lons = zip(*gnss_measurements)
fused_lats, fused_lons = zip(*fused_geopose_measurements)
# Assuming fused_position_measurements is a list of [x, y, z] coordinates
fused_xs, fused_ys, fused_zs = zip(*fused_position_measurements)
fig, axs = plt.subplots(2)
# Plot lat, lon from gnss_measurements and fused_geopose_measurements
axs[0].plot(gnss_lats, gnss_lons, label='GNSS Measurements',color='r')
axs[0].plot(fused_lats, fused_lons, label='Fused Geopose Measurements',color='b')
axs[0].set_xlabel('Latitude')
axs[0].set_ylabel('Longitude')
axs[0].legend()
# Plot fused_position_measurements
axs[1].plot(fused_xs, fused_ys, label='Fused Position Measurements')
axs[1].set_xlabel('X')
axs[1].set_ylabel('Y')
axs[1].legend()
plt.show()
