GNSS Covariance Issue

Software Python
1

I am attempting to achieve GNSS calibration in order to produce geo-rectified positions for predictions from detectron2. Despite varying the position covariances with randomized values, when they are ingested the fusion module returns “GNSS DATA COVARIANCE MUST VARY”

Data: test_pk - Google Drive

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 

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()
    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"])
    covariances[0] += 4*np.random.random((1,))[0]
    covariances[4] += 4*np.random.random((1,))[0]
    covariances[8] += 4*np.random.random((1,))[0]
    gps_data.position_covariances = [float(x)**.5 for x in covariances]
    return gps_data

if __name__ == "__main__":
    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 = 1
    fusion.init(init_fusion_parameters)
    positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
    positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_params
    fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
    
    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
    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 = zed.get_timestamp(sl.TIME_REFERENCE.IMAGE)
        last_timestamp = sl.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()
            print(gnss_data.position_covariances)
            print(f"Camera-GNSS time difference [s]: {time_delta/1e6}")
            status = fusion.ingest_gnss_data(gnss_data)
            if status.name == "SUCCESS":
                print("Ingested GNSS data")
            else:
                print(status)

        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)
                text_rotation = str((round(rotation[0], 2), round(rotation[1], 2), round(rotation[2], 2)))
                text_translation = str((round(translation.get()[0], 2), round(translation.get()[1], 2), round(translation.get()[2], 2)))
                pose_data = odometry_pose.pose_data(sl.Transform())
                print("get position translation  = ",text_translation,", rotation_message = ",text_rotation)

                # get geopose
                fusion.get_geo_pose(current_geopose)
                current_latlng = current_geopose.latlng_coordinates
                print("Lat/Lon = ",current_latlng.get_coordinates(False))
                print("Heading = ",current_geopose.heading)
                gnss_status, yaw_unc,pos_unc = fusion.get_current_gnss_calibration_std()
                print(f"Translation Uncertainty = {pos_unc}")
                print(f"Yaw Uncertainty = {yaw_unc}")
                print(f"GNSS Status = {gnss_status}")

        i = i + 1
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    zed.close()

Terminal:

[2024-05-07 19:17:17 UTC][ZED][INFO] Logging level INFO
[2024-05-07 19:17:17 UTC][ZED][INFO] [Init]  Depth mode: PERFORMANCE
[2024-05-07 19:17:17 UTC][ZED][INFO] [Init]  Serial Number: S/N 47733007
External SVOData channels: ['GNSS']
Subscribing to 47733007 COMM_TYPE.INTRA_PROCESS
[5.9722080993062905, 0.0, 0.0, 0.0, 7.019680286243853, 0.0, 0.0, 0.0, 7.493503332753606]
Camera-GNSS time difference [s]: 0.444982
INVALID TIMESTAMP
[6.194840942204799, 0.0, 0.0, 0.0, 7.023651622792437, 0.0, 0.0, 0.0, 7.585296382575726]
Camera-GNSS time difference [s]: 0.844801
INVALID TIMESTAMP
[6.171552139034422, 0.0, 0.0, 0.0, 6.964669227777214, 0.0, 0.0, 0.0, 7.68532695876349]
Camera-GNSS time difference [s]: 1.128145
GNSS DATA COVARIANCE MUST VARY
[6.075807473616566, 0.0, 0.0, 0.0, 6.932670192297998, 0.0, 0.0, 0.0, 7.331001010091511]
Camera-GNSS time difference [s]: 0.128145
GNSS DATA COVARIANCE MUST VARY
[5.911707914198045, 0.0, 0.0, 0.0, 6.962864443731021, 0.0, 0.0, 0.0, 7.684549962226894]
Camera-GNSS time difference [s]: 1.444801
GNSS DATA COVARIANCE MUST VARY
[6.0248786292402094, 0.0, 0.0, 0.0, 6.921640656722431, 0.0, 0.0, 0.0, 7.320255678483056]
Camera-GNSS time difference [s]: 0.444801
GNSS DATA COVARIANCE MUST VARY
get position translation  =  (0.0, 0.0, 0.0) , rotation_message =  (-0.07, 0.0, -0.1)
Lat/Lon =  (0.0, 0.44762327744595565, 0.0)
Heading =  0.0
Translation Uncertainty = [0. 0. 0.]
Yaw Uncertainty = -3.8337059698629783e-38
GNSS Status = CALIBRATION_IN_PROGRESS
[6.096176117576387, 0.0, 0.0, 0.0, 7.166065866210921, 0.0, 0.0, 0.0, 7.674799407766699]
Camera-GNSS time difference [s]: 1.678267
GNSS DATA COVARIANCE MUST VARY
[6.085925853881649, 0.0, 0.0, 0.0, 7.057691089006473, 0.0, 0.0, 0.0, 7.33464524886549]
Camera-GNSS time difference [s]: 0.678267
GNSS DATA COVARIANCE MUST VARY
get position translation  =  (-0.0, 0.0, -0.0) , rotation_message =  (-0.07, 0.0, -0.1)
Lat/Lon =  (0.0, 0.44762327744595565, 0.0)
Heading =  0.0
Translation Uncertainty = [0. 0. 0.]
Yaw Uncertainty = -0.00011822805390693247
GNSS Status = CALIBRATION_IN_PROGRESS
[6.12171539494147, 0.0, 0.0, 0.0, 7.141807969267532, 0.0, 0.0, 0.0, 7.584266773765523]
Camera-GNSS time difference [s]: 1.928095
GNSS DATA COVARIANCE MUST VARY
[6.181952663130769, 0.0, 0.0, 0.0, 7.186098985318484, 0.0, 0.0, 0.0, 7.278686166734894]
Camera-GNSS time difference [s]: 0.928095
GNSS DATA COVARIANCE MUST VARY
get position translation  =  (-0.0, 0.0, -0.0) , rotation_message =  (-0.07, 0.01, -0.1)
Lat/Lon =  (0.0, 0.44762327744595565, 0.0)
Heading =  0.0
Translation Uncertainty = [0. 0. 0.]
Yaw Uncertainty = -0.00011822805390693247
GNSS Status = CALIBRATION_IN_PROGRESS
2

Hi @fdunbar
What version of the ZED SDK are you using?
Can you test if the same problem is present in the latest v4.1.1?

3

hey @Myzhar, I updated the sdk to 4.1.1

I am still unable to achieve GNSS calibration, and my vio-gnss translations become large (>10m) with large poistional uncertainty (>1e2) with this 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

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"])
    # covariances[0] += 4*np.random.random((1,))[0]
    # covariances[4] += 4*np.random.random((1,))[0]
    # covariances[8] += 4*np.random.random((1,))[0]
    gps_data.position_covariances = [float(x)**.5 for x in covariances]
    return gps_data

if __name__ == "__main__":
    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 = 1
    fusion.init(init_fusion_parameters)
    positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
    positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_params
    fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
    
    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
    last_timestamp = sl.Timestamp()
    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 = zed.get_timestamp(sl.TIME_REFERENCE.IMAGE)

        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()
            print(gnss_data.position_covariances)
            print(f"Camera-GNSS time difference [s]: {time_delta/1e6}")
            status = fusion.ingest_gnss_data(gnss_data)
            if status.name == "SUCCESS":
                print("Ingested GNSS data")
            else:
                print(status)

        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)
                text_rotation = str((round(rotation[0], 2), round(rotation[1], 2), round(rotation[2], 2)))
                text_translation = str((round(translation.get()[0], 2), round(translation.get()[1], 2), round(translation.get()[2], 2)))
                pose_data = odometry_pose.pose_data(sl.Transform())
                print("get position translation  = ",text_translation,", rotation_message = ",text_rotation)

                # get geopose
                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

                if gnss_status.name == "OK":
                   
            
                    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()
4

Hi is anyone still following this? @mattrouss @alassagne @Myzhar

5

Hello @fdunbar
I was not able to reproduce the issue. Could reproduce issue with geotracking playback sample ?

Additionally, I would like to bring to your attention that starting from version 4.1, the Fusion rejects all GNSSData that do not include the status and mode attributes. You can find examples of this in the official sample.

Regards,
Tanguy