diff --git a/event/algorithm/localisation/SphericalIntersection.py b/event/algorithm/localisation/SphericalIntersection.py index 0b2ccd8..20ce84f 100644 --- a/event/algorithm/localisation/SphericalIntersection.py +++ b/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 = assoc_detections - #S = np.zeros() + nDetections = len(assoc_detections[target]) + # 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 \ No newline at end of file diff --git a/event/event.py b/event/event.py index 93af651..bbe21bd 100644 --- a/event/event.py +++ b/event/event.py @@ -141,32 +141,34 @@ async def event(): # show ellipsoids of associated detections for 1 target ellipsoids = {} - if associated_dets_2_radars: - # get first target key - key = next(iter(associated_dets_2_radars)) - ellipsoid_radars = [] - for radar in associated_dets_2_radars[key]: - ellipsoid_radars.append(radar["radar"]) - x_tx, y_tx, z_tx = Geometry.lla2ecef( - radar_dict_item[radar["radar"]]["config"]['location']['tx']['latitude'], - radar_dict_item[radar["radar"]]["config"]['location']['tx']['longitude'], - radar_dict_item[radar["radar"]]["config"]['location']['tx']['altitude'] - ) - x_rx, y_rx, z_rx = Geometry.lla2ecef( - radar_dict_item[radar["radar"]]["config"]['location']['rx']['latitude'], - radar_dict_item[radar["radar"]]["config"]['location']['rx']['longitude'], - radar_dict_item[radar["radar"]]["config"]['location']['rx']['altitude'] - ) - ellipsoid = Ellipsoid( - [x_tx, y_tx, z_tx], - [x_rx, y_rx, z_rx], - radar["radar"] - ) - points = localisation.sample(ellipsoid, radar["delay"]*1000, 50) - for i in range(len(points)): - lat, lon, alt = Geometry.ecef2lla(points[i][0], points[i][1], points[i][2]) - points[i] = ([round(lat, 3), round(lon, 3), 0]) - ellipsoids[radar["radar"]] = points + 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)) + ellipsoid_radars = [] + for radar in associated_dets_2_radars[key]: + ellipsoid_radars.append(radar["radar"]) + x_tx, y_tx, z_tx = Geometry.lla2ecef( + radar_dict_item[radar["radar"]]["config"]['location']['tx']['latitude'], + radar_dict_item[radar["radar"]]["config"]['location']['tx']['longitude'], + radar_dict_item[radar["radar"]]["config"]['location']['tx']['altitude'] + ) + x_rx, y_rx, z_rx = Geometry.lla2ecef( + radar_dict_item[radar["radar"]]["config"]['location']['rx']['latitude'], + radar_dict_item[radar["radar"]]["config"]['location']['rx']['longitude'], + radar_dict_item[radar["radar"]]["config"]['location']['rx']['altitude'] + ) + ellipsoid = Ellipsoid( + [x_tx, y_tx, z_tx], + [x_rx, y_rx, z_rx], + radar["radar"] + ) + points = localisation.sample(ellipsoid, radar["delay"]*1000, 50) + for i in range(len(points)): + lat, lon, alt = Geometry.ecef2lla(points[i][0], points[i][1], points[i][2]) + points[i] = ([round(lat, 3), round(lon, 3), 0]) + ellipsoids[radar["radar"]] = points # output data to API item["timestamp_event"] = timestamp diff --git a/script/plot_accuracy.py b/script/plot_accuracy.py index c248696..974b8ad 100644 --- a/script/plot_accuracy.py +++ b/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() diff --git a/script/plot_associate.py b/script/plot_associate.py new file mode 100644 index 0000000..ecdf0a4 --- /dev/null +++ b/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()