Add sai-cli diagnose tool

python/cli/calibrate/calibrate.py

@@ -14,6 +14,9 @@ def define_subparser(subparsers):
     sub = subparsers.add_parser('calibrate', help=__doc__.strip())
     sub.set_defaults(func=call_calibrate)
     from spectacularAI.calibration import define_args as define_args_calibration
-    from .report import define_args as define_args_report
-    define_args_calibration(sub)
-    define_args_report(sub)
+    try:
+        from .report import define_args as define_args_report
+        define_args_calibration(sub)
+        define_args_report(sub)
+    except:
+        pass

python/cli/diagnose/diagnose.py

@@ -0,0 +1,161 @@
+"""
+Visualize and diagnose common issues in data in Spectacular AI format
+"""
+
+import json
+import pathlib
+import os
+
+from .html import generateHtml
+from .sensors import *
+from .gnss import GnssConverter
+
+def define_args(parser):
+    parser.add_argument("dataset_path", type=pathlib.Path, help="Path to dataset")
+    parser.add_argument("--output_html", type=pathlib.Path, help="Path to calibration report HTML output.")
+    parser.add_argument("--output_json", type=pathlib.Path, help="Path to JSON output.")
+    parser.add_argument("--zero", help="Rescale time to start from zero", action='store_true')
+    parser.add_argument("--skip", type=float, help="Skip N seconds from the start")
+    parser.add_argument("--max", type=float, help="Plot max N seconds from the start")
+    return parser
+
+def define_subparser(subparsers):
+    sub = subparsers.add_parser('diagnose', help=__doc__.strip())
+    sub.set_defaults(func=generateReport)
+    return define_args(sub)
+
+def generateReport(args):
+    from datetime import datetime
+
+    datasetPath = args.dataset_path
+    jsonlFile = datasetPath if datasetPath.suffix == ".jsonl" else datasetPath.joinpath("data.jsonl")
+    if not jsonlFile.is_file():
+        raise FileNotFoundError(f"{jsonlFile} does not exist")
+
+    output = {
+        'passed': True,
+        'date': datetime.now().strftime("%Y-%m-%d %H:%M:%S"),
+        'dataset_path': str(jsonlFile.parent)
+    }
+
+    if not args.output_html and not args.output_json:
+        print("Either --output_html or --output_json is required")
+        return
+
+    data = {
+        'accelerometer': {"v": [], "t": [], "td": []},
+        'gyroscope': {"v": [], "t": [], "td": []},
+        'magnetometer': {"v": [], "t": [], "td": []},
+        'barometer': {"v": [], "t": [], "td": []},
+        'gps': {"v": [], "t": [], "td": []},
+        'cpu': {"v": [], "t": []},
+        'cameras': {}
+    }
+
+    def addMeasurement(type, t, v):
+        assert type in data, f"Unknown sensor type: {type}"
+        sensorData = data[type]
+        sensorData['v'].append(v)
+        if len(sensorData["t"]) > 0:
+            diff = t - sensorData["t"][-1]
+            sensorData["td"].append(diff)
+        sensorData["t"].append(t)
+
+    startTime = None
+    timeOffset = 0
+    gnssConverter = GnssConverter()
+
+    with open(jsonlFile) as f:
+        nSkipped = 0
+        for line in f.readlines():
+            try:
+                measurement = json.loads(line)
+            except:
+                print(f"Warning: ignoring non JSON line: '{line}'")
+                continue
+            time = measurement.get("time")
+            sensor = measurement.get("sensor")
+            barometer = measurement.get("barometer")
+            gps = measurement.get("gps")
+            frames = measurement.get("frames")
+            metrics = measurement.get("systemMetrics")
+            if frames is None and 'frame' in measurement:
+                frames = [measurement['frame']]
+                frames[0]['cameraInd'] = 0
+
+            if time is None: continue
+
+            if (sensor is None
+                and frames is None
+                and metrics is None
+                and barometer is None
+                and gps is None): continue
+
+            if startTime is None:
+                startTime = time
+                if args.zero:
+                    timeOffset = startTime
+
+
+            if (args.skip is not None and time - startTime < args.skip) or (args.max is not None and time - startTime > args.max):
+                nSkipped += 1
+                continue
+
+            t = time - timeOffset
+            if sensor is not None:
+                measurementType = sensor["type"]
+                if measurementType in ["accelerometer", "gyroscope", "magnetometer"]:
+                    v = [sensor["values"][i] for i in range(3)]
+                    addMeasurement(measurementType, t, v)
+            elif barometer is not None:
+                addMeasurement("barometer", t, barometer["pressureHectopascals"])
+            elif gps is not None:
+                enu = gnssConverter.enu(gps["latitude"], gps["longitude"], gps["altitude"])
+                addMeasurement("gps", t, [enu["x"], enu["y"], gps["altitude"]])
+            elif frames is not None:
+                for f in frames:
+                    if f.get("missingBitmap", False): continue
+                    cameras = data['cameras']
+                    ind = f["cameraInd"]
+                    if cameras.get(ind) is None:
+                        cameras[ind] = {"td": [], "t": [], "features": []}
+                    else:
+                        diff = t - cameras[ind]["t"][-1]
+                        cameras[ind]["td"].append(diff)
+                    if "features" in f: cameras[ind]["features"].append(len(f["features"]))
+                    cameras[ind]["t"].append(t)
+            elif metrics is not None and 'cpu' in metrics:
+                data["cpu"]["t"].append(t)
+                data["cpu"]["v"].append(metrics['cpu'].get('systemTotalUsagePercent', 0))
+
+        if nSkipped > 0: print(f'Skipped {nSkipped} lines')
+
+    diagnoseCamera(data, output)
+    diagnoseAccelerometer(data, output)
+    diagnoseGyroscope(data, output)
+    diagnoseMagnetometer(data, output)
+    diagnoseBarometer(data, output)
+    diagnoseGps(data, output)
+    diagnoseCpu(data, output)
+
+    if args.output_json:
+        if os.path.dirname(args.output_json):
+            os.makedirs(os.path.dirname(args.output_json), exist_ok=True)
+        with open(args.output_json, "w") as f:
+            f.write(json.dumps(output, indent=4))
+        print("Generated JSON report data at:", args.output_json)
+
+    if args.output_html:
+        if os.path.dirname(args.output_html):
+            os.makedirs(os.path.dirname(args.output_html), exist_ok=True)
+        generateHtml(output, args.output_html)
+        print("Generated HTML report at:", args.output_html)
+
+if __name__ == '__main__':
+    def parse_args():
+        import argparse
+        parser = argparse.ArgumentParser(description=__doc__.strip())
+        parser = define_args(parser)
+        return parser.parse_args()
+
+    generateReport(parse_args())

python/cli/diagnose/gnss.py

@@ -0,0 +1,112 @@
+import numpy as np
+
+class Ellipsoid:
+    def __init__(self, a, b):
+        self.a = a # semi-major axis
+        self.b = b # semi-minor axis
+        f = 1.0 - b / a # flattening factor
+        self.e2 = 2 * f - f ** 2 # eccentricity squared
+
+class GnssConverter:
+    def __init__(self):
+        self.ell = Ellipsoid(a=6378137.0, b=6356752.31424518) # WGS-84 ellipsoid
+        self.initialized = False
+        self.originECEF = None
+        self.R_ecef2enu = None
+        self.R_enu2ecef = None
+        self.prev = {"x": 0, "y": 0, "z": 0}
+
+    def set_origin(self, lat, lon, alt):
+        def ecef_to_enu_rotation_matrix(lat, lon):
+            lat = np.deg2rad(lat)
+            lon = np.deg2rad(lon)
+
+            return np.array([
+                [-np.sin(lon), np.cos(lon), 0],
+                [-np.sin(lat)*np.cos(lon), -np.sin(lat)*np.sin(lon), np.cos(lat)],
+                [np.cos(lat)*np.cos(lon), np.cos(lat)*np.sin(lon), np.sin(lat)]
+            ])
+
+        self.initialized = True
+        self.originECEF = self.__geodetic2ecef(lat, lon, alt)
+        self.R_ecef2enu = ecef_to_enu_rotation_matrix(lat, lon)
+        self.R_enu2ecef = self.R_ecef2enu.T
+
+    def __geodetic2ecef(self, lat, lon, alt):
+        # https://gssc.esa.int/navipedia/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
+        lat = np.deg2rad(lat)
+        lon = np.deg2rad(lon)
+        a = self.ell.a
+        e2 = self.ell.e2
+        N = a / np.sqrt(1 - e2 * np.sin(lat) * np.sin(lat)) # radius of curvature in the prime vertical
+
+        x = (N + alt) * np.cos(lat) * np.cos(lon)
+        y = (N + alt) * np.cos(lat) * np.sin(lon)
+        z = ((1 - e2) * N + alt) * np.sin(lat)
+        return np.array([x, y, z])
+
+    def __ecef2geodetic(self, x, y, z):
+        # https://gssc.esa.int/navipedia/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
+        a = self.ell.a
+        e2 = self.ell.e2
+        p = np.sqrt(x**2 + y**2)
+        lon = np.arctan2(y, x)
+
+        # latitude and altitude are computed by an iterative procedure.
+        MAX_ITERS = 1000
+        MIN_LATITUDE_CHANGE_RADIANS = 1e-10
+        MIN_ALTITUDE_CHANGE_METERS = 1e-6
+        lat_prev = np.arctan(z / ((1-e2)*p)) # initial value
+        alt_prev = -100000 # arbitrary
+        for _ in range(MAX_ITERS):
+            N_i = a / np.sqrt(1-e2*np.sin(lat_prev)**2)
+            alt_i = p / np.cos(lat_prev) - N_i
+            lat_i = np.arctan(z / ((1 - e2 * (N_i/(N_i + alt_i)))*p))
+            if abs(lat_i - lat_prev) < MIN_LATITUDE_CHANGE_RADIANS and abs(alt_i - alt_prev) < MIN_ALTITUDE_CHANGE_METERS: break
+            alt_prev = alt_i
+            lat_prev = lat_i
+
+        lat = np.rad2deg(lat_i)
+        lon = np.rad2deg(lon)
+        return np.array([lat, lon, alt_i])
+
+    def __ecef2enu(self, x, y, z):
+        # https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
+        assert(self.initialized)
+        xyz = np.array([x, y, z])
+        xyz = xyz - self.originECEF
+        return self.R_ecef2enu @ xyz
+
+    def __enu2ecef(self, e, n, u):
+        # https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
+        assert(self.initialized)
+        enu = np.array([e, n, u])
+        xyz = self.R_enu2ecef @ enu
+        return xyz + self.originECEF
+
+    def enu(self, lat, lon, alt=0, accuracy=1.0, minAccuracy=-1.0):
+         # Filter out inaccurate measurements to make pose alignment easier.
+        if (minAccuracy > 0.0 and (accuracy > minAccuracy or accuracy < 0.0)):
+            return self.prev
+
+        if not self.initialized:
+            self.set_origin(lat, lon, alt)
+
+        x, y, z = self.__geodetic2ecef(lat, lon, alt)
+        enu = self.__ecef2enu(x, y, z)
+        enu = { "x": enu[0], "y": enu[1], "z": enu[2] }
+        self.prev = enu
+        return enu
+
+    def wgs(self, e, n, u):
+        assert(self.initialized)
+        x, y, z = self.__enu2ecef(e, n, u)
+        wgs = self.__ecef2geodetic(x, y, z)
+        return { "latitude": wgs[0], "longitude": wgs[1], "altitude": wgs[2] }
+
+    def wgs_array(self, pos):
+        assert(self.initialized)
+        arr = []
+        for enu in pos:
+            arr.append(self.wgs(enu[0], enu[1], enu[2]))
+        return arr