Skip to content

Diagnose ground truth GNSS #81

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Aug 13, 2025
Merged

Diagnose ground truth GNSS #81

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
58125bc
Diagnose ground truth GNSS.
pekkaran Aug 13, 2025
008fc5e
Show all ground truths in position plot.
pekkaran Aug 13, 2025
14e5207
Plot ground truth and GNSS together in the GNSS section.
pekkaran Aug 13, 2025
7b5d1cf
Fix velocity graph colors.
pekkaran Aug 13, 2025
8b9601f
Convert external pose to GNSS.
pekkaran Aug 13, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 35 additions & 9 deletions python/cli/diagnose/diagnose.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,16 @@ def generateReport(args):
'barometer': {"v": [], "t": [], "td": []},
'cpu': {"v": [], "t": [], "td": [], "processes": {}},
'gnss': {
"name": "GNSS",
"color": "darkred",
"t": [],
"td": [],
"position": [], # ENU
"altitude": [] # WGS-84
},
'globalGroundTruth': {
"name": "Ground truth",
"color": "tab:orange",
"t": [],
"td": [],
"position": [], # ENU
Expand Down Expand Up @@ -85,7 +95,6 @@ def addMeasurement(type, t, v):
time = measurement.get("time")
sensor = measurement.get("sensor")
barometer = measurement.get("barometer")
gnss = measurement.get("gps")
frames = measurement.get("frames")
metrics = measurement.get("systemMetrics")
vioOutput = measurement if "status" in measurement else None
Expand All @@ -94,13 +103,23 @@ def addMeasurement(type, t, v):
frames = [measurement['frame']]
frames[0]['cameraInd'] = 0

gnss = measurement.get("gps")
groundTruth = measurement.get("groundTruth")
externalPose = measurement.get("externalPose")
if "pose" in measurement:
if measurement["pose"]["name"] == "groundTruth": groundTruth = measurement["pose"]
if measurement["pose"]["name"] == "gps": gnss = measurement["pose"]
if measurement["pose"]["name"] == "externalPose": externalPose = measurement["pose"]
if gnss is None: gnss = externalPose

if time is None: continue

if (sensor is None
and frames is None
and metrics is None
and barometer is None
and gnss is None
and groundTruth is None
and vioOutput is None
and droppedFrame is None): continue

Expand All @@ -113,6 +132,16 @@ def addMeasurement(type, t, v):
nSkipped += 1
continue

def convertGnss(gnss, gnssData):
if len(gnssData["t"]) > 0:
diff = t - gnssData["t"][-1]
gnssData["td"].append(diff)
gnssData["t"].append(t)
if "latitude" not in gnss: return
enu = gnssConverter.enu(gnss["latitude"], gnss["longitude"], gnss["altitude"])
gnssData["position"].append([enu[c] for c in "xyz"])
gnssData["altitude"].append(gnss["altitude"])

t = time - timeOffset
if sensor is not None:
measurementType = sensor["type"]
Expand All @@ -129,14 +158,11 @@ def addMeasurement(type, t, v):
addMeasurement("barometer", t, barometer["pressureHectopascals"])

elif gnss is not None:
gnssData = data["gnss"]
if len(gnssData["t"]) > 0:
diff = t - gnssData["t"][-1]
gnssData["td"].append(diff)
gnssData["t"].append(t)
enu = gnssConverter.enu(gnss["latitude"], gnss["longitude"], gnss["altitude"])
gnssData["position"].append([enu[c] for c in "xyz"])
gnssData["altitude"].append(gnss["altitude"])
convertGnss(gnss, data["gnss"])

elif groundTruth is not None:
convertGnss(groundTruth, data["globalGroundTruth"])
# Should also convert non-GNSS ground truth.

elif frames is not None:
for f in frames:
Expand Down
95 changes: 59 additions & 36 deletions python/cli/diagnose/sensors.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,14 @@ def base64(fig):
buf.seek(0)
return base64.b64encode(buf.getvalue()).decode('utf-8')

def getGroundTruths(data):
# Preferred ground truth type first.
groundTruths = []
for field in ["globalGroundTruth", "gnss"]:
if len(data[field]["t"]) > 0:
groundTruths.append(data[field])
return groundTruths

def plotFrame(
x,
ys,
Expand Down Expand Up @@ -579,18 +587,18 @@ def computeVelocity(data, intervalSeconds=None):

plt.subplot(3, 1, 1)
plt.plot(timestamps, velocity[:, 0], label="VIO")
plt.plot(gtTime, gtVelocity[:, 0], label="GNSS")
plt.plot(gtTime, gtVelocity[:, 0], label=groundTruth["name"], color=groundTruth["color"])
plt.ylabel("East (m/s)")
plt.legend()

plt.subplot(3, 1, 2)
plt.plot(timestamps, velocity[:, 1], label="VIO")
plt.plot(gtTime, gtVelocity[:, 1], label="GNSS")
plt.plot(gtTime, gtVelocity[:, 1], label=groundTruth["name"], color=groundTruth["color"])
plt.ylabel("North (m/s)")

plt.subplot(3, 1, 3)
plt.plot(timestamps, velocity[:, 2], label="VIO (z)")
plt.plot(gtTime, gtVelocity[:, 2], label="GNSS")
plt.plot(gtTime, gtVelocity[:, 2], label=groundTruth["name"], color=groundTruth["color"])
plt.ylabel("Up (m/s)")

fig.suptitle(f"VIO velocity in ENU coordinates")
Expand All @@ -603,8 +611,8 @@ def analyzeVIOPosition(
altitudeWGS84,
timestamps,
trackingStatus,
groundTruth):
if groundTruth is None:
groundTruths):
if len(groundTruths) == 0:
self.images.append(plotFrame(
positionENU[:, 0],
positionENU[:, 1],
Expand All @@ -630,16 +638,16 @@ def analyzeVIOPosition(

fig, _ = plt.subplots(2, 1, figsize=(8, 6))

gtPosition = np.array(groundTruth["position"])
gtAltitudeWGS84 = np.array(groundTruth["altitude"])
gtTime = np.array(groundTruth["t"])

# Get lost tracking indices
lostIndices = [i for i, status in enumerate(trackingStatus) if status == "LOST_TRACKING"]

ax1 = plt.subplot(2, 1, 1)
ax1.plot(positionENU[:, 0], positionENU[:, 1], label="VIO")
ax1.plot(gtPosition[:, 0], gtPosition[:, 1], label="GNSS")

for groundTruth in groundTruths:
gtTime = np.array(groundTruth["t"])
gtPosition = np.array(groundTruth["position"])
ax1.plot(gtPosition[:, 0], gtPosition[:, 1], label=groundTruth["name"], color=groundTruth["color"])

# Plot lost tracking points
if lostIndices:
Expand All @@ -656,7 +664,12 @@ def analyzeVIOPosition(

ax2 = plt.subplot(2, 1, 2)
ax2.plot(timestamps, altitudeWGS84, label="VIO")
ax2.plot(gtTime, gtAltitudeWGS84, label="GNSS")

for groundTruth in groundTruths:
gtTime = np.array(groundTruth["t"])
gtAltitudeWGS84 = np.array(groundTruth["altitude"])
ax2.plot(gtTime, gtAltitudeWGS84, label=groundTruth["name"], color=groundTruth["color"])

ax2.set_title("VIO altitude in WGS-84 coordinates")
ax2.set_xlabel("Time (s)")
ax2.set_ylabel("Altitude (m)")
Expand Down Expand Up @@ -994,8 +1007,6 @@ def diagnoseGNSS(data, output):
sensor = data["gnss"]
timestamps = np.array(sensor["t"])
deltaTimes = np.array(sensor["td"])
positionEnu = np.array(sensor["position"])
altitude = np.array(sensor["altitude"])

if len(timestamps) == 0: return

Expand All @@ -1011,32 +1022,43 @@ def diagnoseGNSS(data, output):
},
allowDataGaps=True,
isOptionalSensor=True)
status.analyzeSignalDuplicateValues(positionEnu)
status.analyzeSignalDuplicateValues(np.array(sensor["position"]))

groundTruths = getGroundTruths(data)

import matplotlib.pyplot as plt
fig, ax = plt.subplots(figsize=(8, 6))
for groundTruth in groundTruths:
p = np.array(groundTruth["position"])
linestyle = "-" if len(groundTruth["t"]) > 0 else "."
ax.plot(p[:, 0], p[:, 1], label=groundTruth["name"], color=groundTruth["color"], linestyle=linestyle)
ax.set_title("GNSS position")
ax.set_xlabel("East (m)")
ax.set_ylabel("North (m)")
ax.legend()
ax.set_xscale("linear")
ax.set_yscale("linear")
ax.set_aspect("equal", adjustable="datalim")
fig.tight_layout()
positionImage = base64(fig)

fig, ax = plt.subplots(figsize=(8, 6))
for groundTruth in groundTruths:
linestyle = "-" if len(groundTruth["t"]) > 0 else "."
ax.plot(groundTruth["t"], groundTruth["altitude"], label=groundTruth["name"], color=groundTruth["color"], linestyle=linestyle)
ax.set_title("GNSS altitude (WGS-84)")
ax.set_xlabel("Time (s)")
ax.set_ylabel("Altitude (m)")
ax.legend()
fig.tight_layout()
altitudeImage = base64(fig)

output["GNSS"] = {
"diagnosis": status.diagnosis.toString(),
"issues": status.serializeIssues(),
"frequency": computeSamplingRate(deltaTimes),
"count": len(timestamps),
"images": [
plotFrame(
positionEnu[:, 0],
positionEnu[:, 1],
"GNSS position",
xLabel="ENU x (m)",
yLabel="ENU y (m)",
style='-' if len(timestamps) > 1 else '.',
yScale="linear",
xScale="linear",
equalAxis=True),
plotFrame(
timestamps,
altitude,
"GNSS altitude (WGS-84)",
xLabel="Time (s)",
yLabel="Altitude (m)",
style='-' if len(timestamps) > 1 else '.')
] + status.images
"images": [positionImage, altitudeImage] + status.images,
}
if status.diagnosis == DiagnosisLevel.ERROR:
output["passed"] = False
Expand Down Expand Up @@ -1094,9 +1116,10 @@ def diagnoseVIO(data, output):

images = []
if hasGlobalOutput:
groundTruth = data["gnss"] # TODO: use groundTruth instead of gnss (if available)
if len(groundTruth["t"]) == 0: groundTruth = None
status.analyzeVIOPosition(positionENU, altitudeWGS84, timestamps, trackingStatus, groundTruth)
groundTruths = getGroundTruths(data)
status.analyzeVIOPosition(positionENU, altitudeWGS84, timestamps, trackingStatus, groundTruths)

groundTruth = groundTruths[0] if len(groundTruths) > 0 else None
status.analyzeVIOVelocity(velocityENU, timestamps, groundTruth)
else:
# TODO: improve relative positioning analysis
Expand Down