Diagnose ground truth GNSS (#81)

Author: pekkaran

python/cli/diagnose/diagnose.py

@@ -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
@@ -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
@@ -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
 
@@ -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"]
@@ -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:

python/cli/diagnose/sensors.py

@@ -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,
@@ -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")
@@ -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],
@@ -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:
@@ -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)")
@@ -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
 
@@ -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
@@ -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