diff --git a/python/cli/diagnose/diagnose.py b/python/cli/diagnose/diagnose.py index 4f91999..e3a8d19 100644 --- a/python/cli/diagnose/diagnose.py +++ b/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,6 +103,15 @@ 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 @@ -101,6 +119,7 @@ def addMeasurement(type, t, v): 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: diff --git a/python/cli/diagnose/sensors.py b/python/cli/diagnose/sensors.py index e9b173a..569571f 100644 --- a/python/cli/diagnose/sensors.py +++ b/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