github-mirrors/3lips
1
0
Fork 0
mirror of https://github.com/30hours/3lips.git synced 2024-11-08 12:25:42 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Plot associate script working

Browse source
This commit is contained in:
30hours 2024-03-11 02:45:03 +00:00
parent 6e1a4ec5d0
commit 52a537f0c4
4 changed files with 214 additions and 31 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

73
event/algorithm/localisation/SphericalIntersection.py
View file

@ -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

54
event/event.py
View file

@ -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

4
script/plot_accuracy.py
View file

@ -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()

114
script/plot_associate.py Normal file
View file

@ -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()