Fusion.get_current_gnss_data() returns null coordinates

Software
1

When attempting to ingest GNSS data, despite getting a SUCCESS status code, im getting the following bug:

        lat = 46.xxxx
        lon = -113.xxxx
        elevation = 879.xx
        gnss_data = sl.GNSSData()
        gnss_data.set_coordinates(lat, lon, elevation, in_radian=False)
        gnss_data.position_covariances = covariance
        gnss_data.ts = ts_sl
        
        status = self.fusion.ingest_gnss_data(gnss_data)
        if status.name == "SUCCESS":
            self.gps_status["count"] += 1
            self.gps_status["last_seen"] = timestamp.isoformat()
            self.gps_status["covariance"] = covariance
            print(f"Fusion: Ingested GPS measurement: {status.name}")
        else:
            print(f"Fusion: Ingested GPS measurement Failure: {status.name}")
        return status

When i pause in the debugger and try the following:

test = sl.GNSSData()
self.fusion.get_current_gnssdata(test)
test.get_coordinates(in_radian=False)

the results is

`(0.0,0.0,0.0``


This is likely a hint as to why i cannot get the the GNSS calibration to work.

Any ideas?
1 Like
2

Hello @Frankie Dunbar,
I reviewed your code and their might be a little typo error in your code. Indeed the current gnss_value is fill out inside tests but you as coordinate on test variable. This variable is likely uninitialized, leading to empty. Maybe your problem is more deep than that. You can use get_current_gnss_calibration_std (documentation) for getting current gnss calibration uncertainty. These value should decrease until reaching your initialization threshold.

I hope this help,
Regards,
Tanguy

3

I fixed the typo in my problem description.
The main thing is despite achieving successfull GNSS ingestion, when i call

get_current_gnss_calibration_std()

I get

GPS CALIBRATION (NOT_CALIBRATED, -1.0, array([         -1,          -1,          -1]))
4

Indeed this is strange. If calibration std is -1 it might mean that fusion module do not process any GNSS data. Could you activate both the SDK verbose and Fusion verbose and give us the output log ? You could activate the SDK verbose thanks to this method of InitParameters and fusion verbose by using the verbose method of InitFusionParameters.

Also last questions. Do you enable the publishing of your zed camera ? Publishing must be enabled thanks to this function. Are you grabing the camera in parallel of fusion.process() ?

Regards,
Tanguy

5

Here is the output of my terminal:

[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 10574 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 9574 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 9574 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 8641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 8641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 7641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 7641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 6641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 6641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 5641 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 5708 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 4708 ms.
[2024-01-10 18:07:50 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking]  differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 4708 ms.
(NOT_CALIBRATED, -1.671866111701782e-21, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -1.671866111701782e-21, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -1.671866111701782e-21, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
Fusion: Failed to process image from camera 24837248
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -1.671866111701782e-21, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
(NOT_CALIBRATED, -39886258176.0, array([          0,           0,           0]))
Fusion: Ingested GPS measurement: SUCCESS
8

*unintentionally used 2 accounts for the same error

9

Update:
I made some modifications for debugging purposes:

        lat = 46.xxxx
        lon = -113.xxxx
        elevation = 879.xx
        gnss_data = sl.GNSSData()
        gnss_data.set_coordinates(lat, lon, elevation, in_radian=False)
        gnss_data.position_covariances =[x for x in  covariance]
        gnss_data.ts = ts_sl
        
        status = self.fusion.ingest_gnss_data(gnss_data)
        self.fusion.get_current_gnss_data(gnss_data)
        if status.name == "SUCCESS":
            print(gnss_data.get_coordinates(in_radian=False))
            print(f"Fusion: Ingested GPS measurement: {status.name}")
            print(self.fusion.get_sender_state())
            print(self.fusion.get_current_gnss_calibration_std())
            print(self.fusion.get_geo_tracking_calibration())
            self.gps_status["count"] += 1
            self.gps_status["last_seen"] = timestamp.isoformat()
            self.gps_status["covariance"] = covariance
            print("\n")
            logging.info(f"Fusion: Ingested GPS measurement: {[lat,lon,elevation]} || {covariance}")
        else:
            pass
      
        return status

Terminal Output:

(2.482383274919115e-309, 3.9718132386377157e-308, 6.93212177730574e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7f9bb879c570>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([         -1,          -1,          -1]))
7F9B1C000C10
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000
10

Hello,
In your debugging sample it seems that you do not call the fusion.process() method. Are you sure that you call fusion.process() ? Without this call the fusion won’t process your GNSS data and won’t initialize anything. Indeed the get_current_gnss_data return the last GNSS data proceed by the fusion. Is it possible to share your full debugging sample ?

11

Method in question:

    def add_gps_measurement(self,
                            lat: float,
                            lon: float,
                            elevation: float,
                            timestamp: datetime.datetime,
                            covariance: np.ndarray = 1*np.eye(3),
                            
                            **kwargs
                            ) -> None:
        """
        Add GPS measurements to the Fusion's GNSS data.

        Args:
            lat (float): Measurement latitude in degrees.
            lon (float): Measurement longitude in degrees.
            elevation (float): Measurement elevation in meters.
            covariance (np.ndarray, optional): Measurement covariance matrix. Defaults to identity matrix.

        Example:
            # Assuming GPSServer object already exists
            gpsserver = GPSServer()
            gpsserver.stream_gps()

            # Create Fusion object and Camera object
            fusion = Fusion(zed_config)
            camera = Camera(zed_config)

            # Subscribe Camera to Fusion
            fusion.subscribe(camera)

            while True:
                # Get GPS measurements from GPSServer
                measurements = gpsserver.get_measurements()
                if measurements is not None:
                    # Add GPS measurements to the Fusion
                    fusion.add_gps_measurement(
                        lat=measurements['lat'],
                        lon=measurements['lon'],
                        elevation=measurements['elevation'],
                        covariance=measurements['covariance']
                    )
        """
        if isinstance(timestamp, str):
            timestamp = datetime.datetime.fromisoformat(timestamp)
        
        self.current_gps.set_coordinates(lat, lon, elevation, in_radian=False)
        
        if isinstance(covariance, np.ndarray):
            covariance = covariance.flatten().tolist()
        
        self.current_gps.position_covariances = covariance
        
        ts_sl = sl.Timestamp()
        ts_sl.set_milliseconds(int(1000 * timestamp.timestamp()))
        #ts_sl.set_seconds(int(timestamp.timestamp()))
        self.current_gps.ts = ts_sl
        
        gnss_data = sl.GNSSData()
        gnss_data.set_coordinates(lat, lon, elevation, in_radian=False)
        gnss_data.position_covariances =[x for x in  covariance]
        gnss_data.ts = ts_sl
        
        status = self.fusion.ingest_gnss_data(gnss_data)
       
        if status.name == "SUCCESS" and self.fusion.process() == sl.FUSION_ERROR_CODE.SUCCESS:
            self.fusion.get_current_gnss_data(gnss_data)
            print(gnss_data.get_coordinates(in_radian=False))
            print(f"Fusion: Ingested GPS measurement: {status.name}")
            print(self.fusion.get_sender_state())
            print(self.fusion.get_current_gnss_calibration_std())
            print(self.fusion.get_geo_tracking_calibration())
            self.gps_status["count"] += 1
            self.gps_status["last_seen"] = timestamp.isoformat()
            self.gps_status["covariance"] = covariance
            print("\n")
            logging.info(f"Fusion: Ingested GPS measurement: {[lat,lon,elevation]} || {covariance}")
        else:
            pass
            #print(f"Fusion: Ingested GPS measurement Failure: {status.name}")
        return status

Terminal Output:

(2.485085380473966e-309, 3.976136614763693e-308, 6.93966747943427e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7fbf4c586b90>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([ -1, -1, -1]))
7FBEA43C0470
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000

CLUSTERING
(2.485085380473966e-309, 3.976136614763693e-308, 6.93966747943427e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7fbf4c585a50>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([ -1, -1, -1]))
7FBEA43C0470
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000

(2.485085380473966e-309, 3.976136614763693e-308, 6.93966747943427e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7fbf4c585a50>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([ -1, -1, -1]))
7FBEA43C0470
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000

(2.485085380473966e-309, 3.976136614763693e-308, 6.93966747943427e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7fbf4c586110>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([ -1, -1, -1]))
7FBEA43C0470
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000

(2.485085380473966e-309, 3.976136614763693e-308, 6.93966747943427e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7fbf4c586b90>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([ -1, -1, -1]))
7FBEA43C0470
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000

(2.485085380473966e-309, 3.976136614763693e-308, 6.93966747943427e-310)
Fusion: Ingested GPS measurement: SUCCESS
{<pyzed.sl.CameraIdentifier object at 0x7fbf4c586250>: SUCCESS}
(NOT_CALIBRATED, -1.0, array([ -1, -1, -1]))
7FBEA43C0470
1.000000 0.000000 0.000000 0.000000
0.000000 1.000000 0.000000 0.000000
0.000000 0.000000 1.000000 0.000000
0.000000 0.000000 0.000000 1.000000

12

Hello,
I review your sample and it seems that the camera does not publish data to fusion. Fusion need camera data to proceed gnss data. You can enable camera data publishing by using start_publishing method. Documentation available here.

13

I am logging fusion subscription.

Logs:

INFO:root:Fusion capture started
INFO:root:GPS capture started
INFO:root:Camera <pyzed.sl.CameraIdentifier object at 0x7f084bc90a30> initialized
INFO:root: SUCCESS Camera 24837248 Started publishing
INFO:root:Subscribed to camera: 24837248
INFO:root:Observer: Positional tracking enabled

Again I am trying to operate fusion/camera functionality in a child process:


class ObservationTask:
    def __init__(self, config,redis_handler, task_name: str = "Base") -> None:
        self.redis_handler = redis_handler
        self.task_name = task_name
        self.is_running = False
        self.config = config

    def loop(self):
        pass

    def start(self):
        self.is_running = True
        self.process = Process(target=self.loop,name=self.task_name)
        self.process.start()
        logging.info(f"{self.task_name} capture started")

    def stop(self):
        self.is_running = False
        self.process.join()
        logging.info(f"{self.task_name} capture stopped")
        print(f"{self.task_name} capture stopped")

class FusionTask(ObservationTask):
    def __init__(self,config,redis_handler, task_name: str = "Fusion") -> None:
        super().__init__(config,redis_handler, task_name)
     
    def loop(self):
        print(f"\n\n\n Starting {self.task_name} Task \n\n\n")
        camera = Camera(self.config)
        observer = Observer(self.config)
        camera.start_publishing()
        observer.subscribe(camera)
        observer.enable_positional_tracking()

        while True and self.is_running:
            try:
                for idx,image in enumerate(observer.capture()):
                    self.redis_handler.add_image(image,idx)
                    self.redis_handler.send_timestamp(image.timestamp)
          
                current_position = observer.update_position()
                self.redis_handler.add_position(current_position)
                message = self.redis_handler.gps_subscriber.get_message()
                if message and message["type"] == "message":
                    data = self.redis_handler.get_gps()
                    if data:
                        observer.add_gps_measurement(**data)
                        #logging.info(f"EVENT: GPS Measurement Added {data}")
                        
            except KeyboardInterrupt:
                self.stop()
                break

class CaptureGPSTask(ObservationTask):
    def __init__(self, config,redis_handler) -> None:
        super().__init__(config,redis_handler, task_name="GPS")
        self.gps_server = GPSDClient(host="127.0.0.1")
        self.is_running = False
   
        self.timezone = None


    def loop(self):
        global timezone
        timezone = None
        while self.is_running:
            try:
                for results in self.gps_server.dict_stream(convert_datetime=True, filter=["TPV","SKY"]):
                    if results.get("lat",None) is not None:
                        try:
                            if timezone is None:
                                timezone = get_timezone_from_lat_lon(results.get("lat"),results.get("lon"))
                            eph = results.get("eph",100)
                            epv = results.get("epv",100)
                            
                            if eph < 100:
                                eph *= eph
                            
                            if epv < 100:
                                epv *= epv
                            covariance = np.diag([eph,eph,epv])
                            coordinates = {
                                "lat": results.get("lat"),
                                "lon": results.get("lon"),
                                "elevation": results.get("alt"),
                                "covariance": covariance.flatten(),
                                "timestamp": datetime.datetime.fromisoformat(str(results.get("time")))
                            }
                            ts = coordinates["timestamp"]

                            self.redis_handler.add_gps(coordinates)
                            time.sleep(2)
                            break
                        except ValueError or TypeError as e:
                            # print(results)
                            # print(e)
                            pass
            except KeyboardInterrupt:
                self.stop()
                break

When I run CaptureGPSTask and FusionTask in sperate processes it does not seem to work, but when i run everything from main:

if __name__ == "__main__":
    logging.basicConfig(level=logging.INFO,filename="debug.log",filemode="w")
    redis_handler = RedisHandler()
    redis_handler.redis_server.flushdb()
    cv2.namedWindow("preview", cv2.WINDOW_NORMAL)


    zed_config = Config(zed_config_path)
    observer = Observer(zed_config)
    camera = Camera(zed_config)

    observer.subscribe(camera)
    observer.enable_positional_tracking()

    gps_server = GPSDClient(host="127.0.0.1")

    while True:
        try:
       
            for idx,image in enumerate(observer.capture()):

                redis_handler.add_image(image,idx)
                redis_handler.send_timestamp(image.timestamp)
            
            current_position = observer.update_position()
            redis_handler.add_position(current_position)

            get_measurement(gps_server)
            observer.add_gps_measurement(**redis_handler.get_gps())


            cv2.imshow("preview", redis_handler.get_image().rgb)
            if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
        except KeyboardInterrupt:
            break
        except Exception:
            pass

    cv2.destroyAllWindows()

I get GNSS calibration within a minute.

14

Any ideas for the most recent response?

15

Just to be sure. Are you using the ZED SDK and the Fusion in different process or in different thread ? If you are using different process you must enable network communication between ZED SDK and Fusion. For doing this you must specify the CommunicationParameters setup for network both in zed.start_publishing and fusion.subscribe. For configuring the CommunicationParameters to be network, you could use set_for_local_network method of CommunicationParameters.

Regards,
Tanguy