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