Plot associate script working

event/algorithm/localisation/SphericalIntersection.py

@@ -3,7 +3,6 @@
 @author 30hours
 """
 
-from data.Ellipsoid import Ellipsoid
 from algorithm.geometry.Geometry import Geometry
 import numpy as np
 import math
@@ -23,6 +22,9 @@ class SphericalIntersection:
         @brief Constructor for the SphericalIntersection class.
         """
 
+        self.type = "rx"
+        self.not_type = "rx" if self.type == "tx" else "tx"
+
     def process(self, assoc_detections, radar_data):
 
         """
@@ -38,10 +40,73 @@ class SphericalIntersection:
         if not assoc_detections:
           return output
 
+        # pick first radar rx node as ENU reference (arbitrary)
+        radar = next(iter(radar_data))
+        reference_lla = [
+          radar_data[radar]["config"][self.type]["latitude"], 
+          radar_data[radar]["config"][self.type]["longitude"],
+          radar_data[radar]["config"][self.type]["altitude"]]
+
         for target in assoc_detections:
 
-            print(assoc_detections)
+            nDetections = len(assoc_detections[target])
-            #nDetections = assoc_detections
+
-            #S = np.zeros()
+            # matrix of positions of non-constant node
+            S = np.zeros((nDetections, 3))
+
+            # additional vector
+            z = np.zeros((nDetections, 1))
+
+            # bistatic range vector r
+            r = np.zeros((nDetections, 1))
+
+            for index, radar in enumerate(assoc_detections[target]):
+
+                # convert position to ENU and add to S
+                config = radar_data[radar["radar"]]["config"]
+                x, y, z = Geometry.lla2ecef(
+                  config['location'][self.type]['latitude'],
+                  config['location'][self.type]['longitude'],
+                  config['location'][self.type]['altitude'])
+                x_enu, y_enu, z_enu = Geometry.ecef2enu(x, y, z, 
+                  reference_lla[0], 
+                  reference_lla[1], 
+                  reference_lla[2])
+                S[index, :] = [x_enu, y_enu, z_enu]
+
+                # add to z
+                x2, y2, z2 = Geometry.lla2ecef(
+                  config['location'][self.not_type]['latitude'],
+                  config['location'][self.not_type]['longitude'],
+                  config['location'][self.not_type]['altitude'])
+                distance = Geometry.distance_ecef([x, y, z], [x2, y2, z2])
+                z[index, :] = (x**2 + y**2 + z**2 - distance**2)/2
+
+                # add to r
+                r[index, :] = radar["delay"] + distance
+
+            # now compute matrix math
+            S_star = np.linalg.inv(S.T @ S) @ S.T
+            a = S_star @ z
+            b = S_star @ r
+            R_t = [0, 0]
+            R_t[0] = (-2*(a.T @ b) - np.sqrt(4*(a.T @ b)**2 - \
+              4*((b.T @ b)-1)*(a.T @ a)))/2*((b.T @ b)-1)
+            R_t[1] = (-2*(a.T @ b) + np.sqrt(4*(a.T @ b)**2 - \
+              4*((b.T @ b)-1)*(a.T @ a)))/2*((b.T @ b)-1)
+            x_t = [0, 0]
+            x_t[0] = S_star @ (z + r*R_t[0])
+            x_t[1] = S_star @ (z + r*R_t[1])
+
+            # use solution with highest altitude
+            output[target] = {}
+            output[target]["points"] = []
+            if x_t[0][2] > x_t[1][2]:
+                output[target]["points"].append(x_t[0])
+            else:
+                output[target]["points"].append(x_t[1])
+
+            print('SX points:')
+            print(x_t)
                 
         return output

event/event.py

@@ -141,6 +141,8 @@ async def event():
 
       # show ellipsoids of associated detections for 1 target
       ellipsoids = {}
+      if item["localisation"] == "ellipse-parametric" or \
+      item["localisation"] == "ellipsoid-parametric":
           if associated_dets_2_radars:
             # get first target key
             key = next(iter(associated_dets_2_radars))

script/plot_accuracy.py

@@ -63,6 +63,7 @@ def main():
     server = json_data[0][0]["server"]
     timestamp = []
     position = {}
+    detected = {}
     truth_timestamp = []
     truth_position = []
     for item in json_data:
@@ -162,7 +163,8 @@ def main():
     plt.subplot(3, 1, 1)
     plt.legend()
     plt.tight_layout()
-    plt.savefig('save/plot_accuracy.png', bbox_inches='tight', pad_inches=0.01)
+    filename = 'plot_accuracy_' + args.target_name + '.png'
+    plt.savefig('save/' + filename, bbox_inches='tight', pad_inches=0.01)
 
 if __name__ == "__main__":
     main()

script/plot_associate.py

@@ -0,0 +1,114 @@
+import argparse
+import json
+import sys
+from datetime import datetime
+import numpy as np
+import matplotlib.pyplot as plt
+
+from geometry.Geometry import Geometry
+
+def parse_posix_time(value):
+    try:
+        return int(value)
+    except ValueError:
+        raise argparse.ArgumentTypeError("Invalid POSIX time format")
+
+def parse_command_line_arguments():
+    parser = argparse.ArgumentParser(description="Process command line arguments.")
+    
+    parser.add_argument("json_file", type=str, help="Input JSON file path")
+    parser.add_argument("target_name", type=str, help="Target name")
+    parser.add_argument("--start_time", type=parse_posix_time, help="Optional start time in POSIX seconds")
+    parser.add_argument("--stop_time", type=parse_posix_time, help="Optional stop time in POSIX seconds")
+
+    return parser.parse_args()
+
+def main():
+  
+    # input handling
+    args = parse_command_line_arguments()
+    json_data = []
+    with open(args.json_file, 'r') as json_file:
+        for line in json_file:
+            try:
+                json_object = json.loads(line)
+                json_data.append(json_object)
+            except json.JSONDecodeError:
+                print(f"Error decoding JSON from line: {line}")
+    json_data = [item for item in json_data if item]
+    start_time = args.start_time if args.start_time else None
+    stop_time = args.stop_time if args.stop_time else None
+    print("JSON String (Last Non-Empty Data):", json_data[-1])
+    print("Target Name:", args.target_name)
+    print("Start Time:", start_time)
+    print("Stop Time:", stop_time)
+
+    # extract data of interest
+    server = json_data[0][0]["server"]
+    timestamp = []
+    associated = {}
+    for item in json_data:
+
+        first_result = item[0]
+
+        if first_result["server"] != server:
+            print('error')
+            sys.exit(-1)
+
+        if start_time and first_result["timestamp_event"]/1000 < start_time:
+            continue
+
+        if stop_time and first_result["timestamp_event"]/1000 > stop_time:
+            continue
+
+        # store association data
+        if "detections_associated" in first_result:
+
+            if args.target_name in first_result["detections_associated"]:
+
+                for radar in first_result["detections_associated"][args.target_name]:
+
+                    if radar['radar'] not in associated:
+                        associated[radar['radar']] = []
+                    else:
+                        associated[radar['radar']].append(first_result["timestamp_event"])
+
+        timestamp.append(first_result["timestamp_event"])
+
+    # data massaging
+    timestamp = list(dict.fromkeys(timestamp))
+    associated = dict(sorted(associated.items(), key=lambda x: x[0]))
+    radars = list(associated.keys())
+    radar_label = []
+    for radar in radars:
+        radar_label.append(radar.split('.', 1)[0])
+
+    # get start and stop times from data
+    start_time = min(min(arr) for arr in associated.values())
+    stop_time = max(max(arr) for arr in associated.values())
+    timestamp = [value for value in timestamp if value >= start_time]
+    timestamp = [value for value in timestamp if value <= stop_time]
+
+    print(associated)
+    
+    data = []
+    for radar in radars:
+        result = [1 if value in associated[radar] else 0 for value in timestamp]
+        data.append(result)
+    
+    print(data)
+
+    # plot x, y, z
+    plt.figure(figsize=(8,4))
+    img = plt.imshow(data, aspect='auto', interpolation='none')
+    y_extent = plt.gca().get_ylim()
+    img.set_extent([start_time/1000, stop_time/1000, y_extent[1], y_extent[0]])
+    plt.yticks(np.arange(len(radar_label)), radar_label, rotation='vertical')
+    plt.xlabel('Timestamp')
+    plt.ylabel('Radar')
+    plt.tight_layout()
+    filename = 'plot_associate_' + args.target_name + '.png'
+    plt.savefig('save/' + filename, bbox_inches='tight', pad_inches=0.01)
+
+if __name__ == "__main__":
+    main()