mirror of
https://github.com/30hours/3lips.git
synced 2024-11-08 12:25:42 +00:00
Various bug fixes and adding mean/min methods
This commit is contained in:
parent
9f31e3190c
commit
129bf2fcc8
7 changed files with 77 additions and 36 deletions
|
@ -25,8 +25,10 @@ associators = [
|
|||
]
|
||||
# todo: ellipse conic int (analytic), SX, arc length
|
||||
localisations = [
|
||||
{"name": "Ellipse Parametric", "id": "ellipse-parametric"},
|
||||
{"name": "Ellipsoid Parametric", "id": "ellipsoid-parametric"},
|
||||
{"name": "Ellipse Parametric (Mean)", "id": "ellipse-parametric-mean"},
|
||||
{"name": "Ellipse Parametric (Min)", "id": "ellipse-parametric-min"},
|
||||
{"name": "Ellipsoid Parametric (Mean)", "id": "ellipsoid-parametric-mean"},
|
||||
{"name": "Ellipsoid Parametric (Min)", "id": "ellipsoid-parametric-min"},
|
||||
{"name": "Spherical Intersection", "id": "spherical-intersection"}
|
||||
]
|
||||
adsbs = [
|
||||
|
|
|
@ -142,6 +142,8 @@ class EllipseParametric:
|
|||
output[target] = {}
|
||||
output[target]["points"] = []
|
||||
for i in range(len(samples_intersect)):
|
||||
print('err??', flush=True)
|
||||
print(samples_intersect, flush=True)
|
||||
samples_intersect[i] = Geometry.ecef2lla(
|
||||
samples_intersect[i][0],
|
||||
samples_intersect[i][1],
|
||||
|
|
|
@ -103,6 +103,9 @@ class EllipsoidParametric:
|
|||
average_point = Geometry.average_points(samples_intersect)
|
||||
samples_intersect = [average_point]
|
||||
|
||||
if len(samples_intersect) == 0:
|
||||
return output
|
||||
|
||||
elif self.method == "minimum":
|
||||
|
||||
min_distance = self.threshold
|
||||
|
|
|
@ -13,7 +13,7 @@ class SphericalIntersection:
|
|||
@class SphericalIntersection
|
||||
@brief A class for intersecting ellipsoids using SX method.
|
||||
@details Uses associated detections from multiple radars.
|
||||
@see blah2 at https://github.com/30hours/blah2.
|
||||
@see https://ieeexplore.ieee.org/document/6129656
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
|
@ -42,9 +42,6 @@ class SphericalIntersection:
|
|||
|
||||
# pick first radar rx node as ENU reference (arbitrary)
|
||||
radar = next(iter(radar_data))
|
||||
print(radar_data)
|
||||
print(radar)
|
||||
print(radar_data[radar]["config"])
|
||||
reference_lla = [
|
||||
radar_data[radar]["config"]["location"][self.not_type]["latitude"],
|
||||
radar_data[radar]["config"]["location"][self.not_type]["longitude"],
|
||||
|
@ -84,24 +81,11 @@ class SphericalIntersection:
|
|||
[x, y, z], [reference_ecef[0],
|
||||
reference_ecef[1], reference_ecef[2]])
|
||||
R_i = (radar["delay"]*1000) + distance
|
||||
# print('R_i', flush=True)
|
||||
# print(R_i, flush=True)
|
||||
# print(radar["delay"]*1000, flush=True)
|
||||
z_vec[index, :] = (x_enu**2 + y_enu**2 + z_enu**2 - R_i**2)/2
|
||||
|
||||
# add to r
|
||||
r[index, :] = R_i
|
||||
|
||||
# print first to check
|
||||
print('start printing SX:', flush=True)
|
||||
print(S, flush=True)
|
||||
print(S.size, flush=True)
|
||||
print(z_vec, flush=True)
|
||||
print(z_vec.size, flush=True)
|
||||
print(r, flush=True)
|
||||
print(r.size, flush=True)
|
||||
|
||||
|
||||
# now compute matrix math
|
||||
S_star = np.linalg.inv(S.T @ S) @ S.T
|
||||
a = S_star @ z_vec
|
||||
|
@ -112,7 +96,6 @@ class SphericalIntersection:
|
|||
R_t[0] = (-2*(a.T @ b) - np.sqrt(discriminant))/(2*((b.T @ b)-1))
|
||||
R_t[1] = (-2*(a.T @ b) + np.sqrt(discriminant))/(2*((b.T @ b)-1))
|
||||
else:
|
||||
print('@@@ discriminant < 0', flush=True)
|
||||
R_t[0] = np.real((-2*(a.T @ b) - np.sqrt(discriminant + 0j))/(2*((b.T @ b)-1)))
|
||||
R_t[1] = np.real((-2*(a.T @ b) + np.sqrt(discriminant + 0j))/(2*((b.T @ b)-1)))
|
||||
x_t = [0, 0]
|
||||
|
@ -123,8 +106,6 @@ class SphericalIntersection:
|
|||
output[target] = {}
|
||||
output[target]["points"] = []
|
||||
x_t_list = [np.squeeze(arr).tolist() for arr in x_t]
|
||||
print('x_t in ENU?')
|
||||
print(x_t_list)
|
||||
|
||||
# convert points back to LLA
|
||||
for index in range(len(x_t_list)):
|
||||
|
@ -141,8 +122,5 @@ class SphericalIntersection:
|
|||
output[target]["points"].append(x_t_list[0])
|
||||
else:
|
||||
output[target]["points"].append(x_t_list[1])
|
||||
|
||||
print('SX points:')
|
||||
print(x_t_list)
|
||||
|
||||
return output
|
|
@ -29,8 +29,10 @@ api = []
|
|||
# init config
|
||||
tDelete = 60
|
||||
adsbAssociator = AdsbAssociator()
|
||||
ellipseParametric = EllipseParametric("mean", 200, 500)
|
||||
ellipsoidParametric = EllipsoidParametric("mean", 100, 500)
|
||||
ellipseParametricMean = EllipseParametric("mean", 150, 500)
|
||||
ellipseParametricMin = EllipseParametric("min", 150, 500)
|
||||
ellipsoidParametricMean = EllipsoidParametric("mean", 120, 1000)
|
||||
ellipsoidParametricMin = EllipsoidParametric("min", 120, 1000)
|
||||
sphericalIntersection = SphericalIntersection()
|
||||
adsbTruth = AdsbTruth(5)
|
||||
save = True
|
||||
|
@ -114,10 +116,14 @@ async def event():
|
|||
return
|
||||
|
||||
# localisation selection
|
||||
if item["localisation"] == "ellipse-parametric":
|
||||
localisation = ellipseParametric
|
||||
elif item["localisation"] == "ellipsoid-parametric":
|
||||
localisation = ellipsoidParametric
|
||||
if item["localisation"] == "ellipse-parametric-mean":
|
||||
localisation = ellipseParametricMean
|
||||
elif item["localisation"] == "ellipse-parametric-min":
|
||||
localisation = ellipseParametricMin
|
||||
elif item["localisation"] == "ellipsoid-parametric-mean":
|
||||
localisation = ellipsoidParametricMean
|
||||
elif item["localisation"] == "ellipsoid-parametric-min":
|
||||
localisation = ellipsoidParametricMin
|
||||
elif item["localisation"] == "spherical-intersection":
|
||||
localisation = sphericalIntersection
|
||||
else:
|
||||
|
@ -143,8 +149,10 @@ async def event():
|
|||
|
||||
# show ellipsoids of associated detections for 1 target
|
||||
ellipsoids = {}
|
||||
if item["localisation"] == "ellipse-parametric" or \
|
||||
item["localisation"] == "ellipsoid-parametric":
|
||||
if item["localisation"] == "ellipse-parametric-mean" or \
|
||||
item["localisation"] == "ellipsoid-parametric-mean" or \
|
||||
item["localisation"] == "ellipse-parametric-min" or \
|
||||
item["localisation"] == "ellipsoid-parametric-min":
|
||||
if associated_dets_2_radars:
|
||||
# get first target key
|
||||
key = next(iter(associated_dets_2_radars))
|
||||
|
@ -169,7 +177,13 @@ async def event():
|
|||
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])
|
||||
if item["localisation"] == "ellipsoid-parametric-mean" or \
|
||||
item["localisation"] == "ellipsoid-parametric-min":
|
||||
alt = round(alt)
|
||||
if item["localisation"] == "ellipse-parametric-mean" or \
|
||||
item["localisation"] == "ellipse-parametric-min":
|
||||
alt = 0
|
||||
points[i] = ([round(lat, 3), round(lon, 3), alt])
|
||||
ellipsoids[radar["radar"]] = points
|
||||
|
||||
stop_time = time.time()
|
||||
|
|
|
@ -36,6 +36,22 @@ def interpolate_positions(timestamp_vector, truth_timestamp, truth_position):
|
|||
|
||||
return interpolated_positions
|
||||
|
||||
def calculate_rmse(actual_values, predicted_values):
|
||||
# Convert lists to NumPy arrays for easy calculations
|
||||
actual_values = np.array(actual_values)
|
||||
predicted_values = np.array(predicted_values)
|
||||
|
||||
# Calculate the squared differences
|
||||
squared_diff = (actual_values - predicted_values) ** 2
|
||||
|
||||
# Calculate the mean squared error
|
||||
mean_squared_error = np.mean(squared_diff)
|
||||
|
||||
# Calculate the root mean squared error
|
||||
rmse = np.sqrt(mean_squared_error)
|
||||
|
||||
return rmse
|
||||
|
||||
def main():
|
||||
|
||||
# input handling
|
||||
|
@ -82,6 +98,7 @@ def main():
|
|||
# store target data
|
||||
method_localisation = method["localisation"]
|
||||
|
||||
# override skip a method
|
||||
if method_localisation == "spherical-intersection":
|
||||
continue
|
||||
|
||||
|
@ -146,13 +163,18 @@ def main():
|
|||
radar4_lla[0], radar4_lla[1], radar4_lla[2]))
|
||||
|
||||
# plot x, y, z
|
||||
plt.figure(figsize=(5,7))
|
||||
#plt.figure(figsize=(5,7))
|
||||
fig, axes = plt.subplots(3, 1, figsize=(5, 7), sharex=True)
|
||||
for i in range(3):
|
||||
yaxis_truth = [pos[i] for pos in truth_position_resampled_enu]
|
||||
plt.subplot(3, 1, i+1)
|
||||
plt.plot(timestamp, yaxis_truth, label="ADS-B Truth")
|
||||
for method in position:
|
||||
print(position[method])
|
||||
if "detections_enu" not in position[method]:
|
||||
continue
|
||||
for i in range(3):
|
||||
print(position)
|
||||
yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
|
||||
plt.subplot(3, 1, i+1)
|
||||
plt.plot(position[method]["timestamp"], yaxis_target, 'x', label=method)
|
||||
|
@ -170,5 +192,25 @@ def main():
|
|||
filename = 'plot_accuracy_' + args.target_name + '.png'
|
||||
plt.savefig('save/' + filename, bbox_inches='tight', pad_inches=0.01)
|
||||
|
||||
# save tabular data
|
||||
table = {}
|
||||
for method in position:
|
||||
if "detections_enu" not in position[method]:
|
||||
continue
|
||||
table[method] = {}
|
||||
for i in range(3):
|
||||
|
||||
yaxis_truth = np.array([pos[i] for pos in truth_position_resampled_enu])
|
||||
matching_indices = np.isin(np.array(timestamp), np.array(position[method]["timestamp"]))
|
||||
yaxis_truth_target = yaxis_truth[matching_indices]
|
||||
|
||||
yaxis_target = [pos[i] for pos in position[method]["detections_enu"]]
|
||||
table[method][str(i)] = calculate_rmse(yaxis_target, yaxis_truth_target)
|
||||
print('test')
|
||||
print(yaxis_target)
|
||||
print(yaxis_truth_target)
|
||||
|
||||
print(table)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
@ -103,7 +103,7 @@ def main():
|
|||
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.yticks(np.arange(len(radar_label)), radar_label[::-1], rotation='vertical')
|
||||
plt.xlabel('Timestamp')
|
||||
plt.ylabel('Radar')
|
||||
plt.tight_layout()
|
||||
|
|
Loading…
Reference in a new issue