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

Fixed ellipse and ellipsoid methoids

Browse source
This commit is contained in:
30hours 2024-03-13 13:04:34 +00:00
parent 21c2d549f3
commit 9f31e3190c
5 changed files with 112 additions and 100 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
event/algorithm/geometry/Geometry.py
View file

@ -189,4 +189,7 @@ class Geometry:
return math.sqrt( return math.sqrt(
(point2[0]-point1[0])**2 + (point2[0]-point1[0])**2 +
(point2[1]-point1[1])**2 + (point2[1]-point1[1])**2 +
(point2[2]-point1[2])**2) (point2[2]-point1[2])**2)
def average_points(points):
return [sum(coord) / len(coord) for coord in zip(*points)]

130
event/algorithm/localisation/EllipseParametric.py
View file

@ -20,15 +20,16 @@ class EllipseParametric:
@see blah2 at https://github.com/30hours/blah2. @see blah2 at https://github.com/30hours/blah2.
""" """
def __init__(self): def __init__(self, method="mean", nSamples=150, threshold=500):
""" """
@brief Constructor for the EllipseParametric class. @brief Constructor for the EllipseParametric class.
""" """
self.ellipsoids = [] self.ellipsoids = []
self.nSamples = 150 self.nSamples = nSamples
self.threshold = 800 self.threshold = threshold
self.method = method
def process(self, assoc_detections, radar_data): def process(self, assoc_detections, radar_data):
@ -87,54 +88,56 @@ class EllipseParametric:
radar_keys = list(target_samples[target].keys()) radar_keys = list(target_samples[target].keys())
samples_intersect = [] samples_intersect = []
# loop points in master ellipsoid if self.method == "mean":
# for point1 in target_samples[target][radar_keys[0]]:
# valid_point = True
# # loop over each other list
# for i in range(1, len(radar_keys)):
# # loop points in other list
# if not any(Geometry.distance_ecef(point1, point2) < self.threshold
# for point2 in target_samples[target][radar_keys[i]]):
# valid_point = False
# break
# if valid_point:
# samples_intersect.append(point1)
# average_point = self.average_points(samples_intersect) # loop points in main ellipsoid
# samples_intersect = [average_point] for point1 in target_samples[target][radar_keys[0]]:
valid_point = True
# loop over each other list
for i in range(1, len(radar_keys)):
# loop points in other list
if not any(Geometry.distance_ecef(point1, point2) < self.threshold
for point2 in target_samples[target][radar_keys[i]]):
valid_point = False
break
if valid_point:
samples_intersect.append(point1)
average_point = Geometry.average_points(samples_intersect)
samples_intersect = [average_point]
elif self.method == "minimum":
min_distance = self.threshold
min_point1 = None
# loop points in main ellipsoid
for point1 in target_samples[target][radar_keys[0]]:
valid_point = True
distance_from_point1 = [self.threshold]*(len(radar_keys)-1)
# loop over each other list
for i in range(1, len(radar_keys)):
if i > 1 and distance_from_point1[i-1] > self.threshold:
valid_point = False
break
# loop points in other list
for point2 in target_samples[target][radar_keys[i]]:
distance = Geometry.distance_ecef(point1, point2)
if distance < distance_from_point1[i-1]:
distance_from_point1[i-1] = distance
norm = math.sqrt(sum(x ** 2 for x in distance_from_point1))
if valid_point and norm < min_distance:
min_distance = norm
min_point1 = point1
if min_point1 is not None:
samples_intersect.append(min_point1)
else:
return output
min_distance = self.threshold
min_point1 = None
for point1 in target_samples[target][radar_keys[0]]:
valid_point = True
distance_from_point1 = [self.threshold]*(len(radar_keys)-1)
# loop over each other list
for i in range(1, len(radar_keys)):
if i > 1 and distance_from_point1[i-1] > self.threshold:
valid_point = False
break
# loop points in other list
for point2 in target_samples[target][radar_keys[i]]:
distance = Geometry.distance_ecef(point1, point2)
if distance < distance_from_point1[i-1]:
distance_from_point1[i-1] = distance
norm = math.sqrt(sum(x ** 2 for x in distance_from_point1))
if valid_point and norm < min_distance:
min_distance = norm
min_point1 = point1
if min_point1 is not None:
samples_intersect.append(min_point1)
else: else:
print('Invalid method.')
return output return output
# find closest points bruteforce
# points = list(target_samples[target].values())
# result_points, result_distance = self.closest_points_bruteforce(points)
# average_point = self.average_points(result_points)
# if result_distance < self.threshold:
# samples_intersect.append(average_point)
# remove duplicates and convert to LLA # remove duplicates and convert to LLA
output[target] = {} output[target] = {}
output[target]["points"] = [] output[target]["points"] = []
@ -190,37 +193,4 @@ class EllipseParametric:
output.append([x, y, z]) output.append([x, y, z])
return output return output
def euclidean_distance(self, point1, point2):
return np.linalg.norm(np.array(point1) - np.array(point2))
def closest_points_bruteforce(self, point_sets):
closest_distance = float('inf')
closest_points = None
for combination in itertools.product(*point_sets):
distance = sum(self.euclidean_distance(combination[i], combination[i+1]) for i in range(len(point_sets)-1))
if distance < closest_distance:
closest_distance = distance
closest_points = combination
return closest_points, closest_distance
# def closest_points_bruteforce(point_sets):
# closest_distance = float('inf')
# closest_points = None
# def calculate_distance(combination):
# nonlocal closest_distance, closest_points
# distance = sum(euclidean_distance(combination[i], combination[i+1]) for i in range(len(point_sets)-1))
# if distance < closest_distance:
# closest_distance = distance
# closest_points = combination
# with ThreadPoolExecutor() as executor:
# executor.map(calculate_distance, itertools.product(*point_sets))
# return closest_points, closest_distance
def average_points(self, points):
return [sum(coord) / len(coord) for coord in zip(*points)]

70
event/algorithm/localisation/EllipsoidParametric.py
View file

@ -17,15 +17,16 @@ class EllipsoidParametric:
@see blah2 at https://github.com/30hours/blah2. @see blah2 at https://github.com/30hours/blah2.
""" """
def __init__(self): def __init__(self, method="mean", nSamples=100, threshold=500):
""" """
@brief Constructor for the EllipsoidParametric class. @brief Constructor for the EllipsoidParametric class.
""" """
self.ellipsoids = [] self.ellipsoids = []
self.nSamples = 150 self.nSamples = nSamples
self.threshold = 800 self.threshold = threshold
self.method = method
def process(self, assoc_detections, radar_data): def process(self, assoc_detections, radar_data):
@ -84,18 +85,55 @@ class EllipsoidParametric:
radar_keys = list(target_samples[target].keys()) radar_keys = list(target_samples[target].keys())
samples_intersect = [] samples_intersect = []
# loop points in master ellipsoid if self.method == "mean":
for point1 in target_samples[target][radar_keys[0]]:
valid_point = True # loop points in main ellipsoid
# loop over each other list for point1 in target_samples[target][radar_keys[0]]:
for i in range(1, len(radar_keys)): valid_point = True
# loop points in other list # loop over each other list
if not any(Geometry.distance_ecef(point1, point2) < self.threshold for i in range(1, len(radar_keys)):
for point2 in target_samples[target][radar_keys[i]]): # loop points in other list
valid_point = False if not any(Geometry.distance_ecef(point1, point2) < self.threshold
break for point2 in target_samples[target][radar_keys[i]]):
if valid_point: valid_point = False
samples_intersect.append(point1) break
if valid_point:
samples_intersect.append(point1)
average_point = Geometry.average_points(samples_intersect)
samples_intersect = [average_point]
elif self.method == "minimum":
min_distance = self.threshold
min_point1 = None
# loop points in main ellipsoid
for point1 in target_samples[target][radar_keys[0]]:
valid_point = True
distance_from_point1 = [self.threshold]*(len(radar_keys)-1)
# loop over each other list
for i in range(1, len(radar_keys)):
if i > 1 and distance_from_point1[i-1] > self.threshold:
valid_point = False
break
# loop points in other list
for point2 in target_samples[target][radar_keys[i]]:
distance = Geometry.distance_ecef(point1, point2)
if distance < distance_from_point1[i-1]:
distance_from_point1[i-1] = distance
norm = math.sqrt(sum(x ** 2 for x in distance_from_point1))
if valid_point and norm < min_distance:
min_distance = norm
min_point1 = point1
if min_point1 is not None:
samples_intersect.append(min_point1)
else:
return output
else:
print('Invalid method.')
return output
# remove duplicates and convert to LLA # remove duplicates and convert to LLA
output[target] = {} output[target] = {}
@ -162,4 +200,4 @@ class EllipsoidParametric:
[x, y, z] = Geometry.lla2ecef(x, y, z) [x, y, z] = Geometry.lla2ecef(x, y, z)
output.append([round(x, 3), round(y, 3), round(z)]) output.append([round(x, 3), round(y, 3), round(z)])
return output return output

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

@ -107,11 +107,12 @@ class SphericalIntersection:
a = S_star @ z_vec a = S_star @ z_vec
b = S_star @ r b = S_star @ r
R_t = [0, 0] R_t = [0, 0]
discrimninant = 4*((a.T @ b)**2) - 4*((b.T @ b) - 1)*(a.T @ a) discriminant = 4*((a.T @ b)**2) - 4*((b.T @ b) - 1)*(a.T @ a)
if discriminant >= 0: if discriminant >= 0:
R_t[0] = (-2*(a.T @ b) - np.sqrt(discriminant))/(2*((b.T @ b)-1)) 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)) R_t[1] = (-2*(a.T @ b) + np.sqrt(discriminant))/(2*((b.T @ b)-1))
else: 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[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))) R_t[1] = np.real((-2*(a.T @ b) + np.sqrt(discriminant + 0j))/(2*((b.T @ b)-1)))
x_t = [0, 0] x_t = [0, 0]

4
event/event.py
View file

@ -29,8 +29,8 @@ api = []
# init config # init config
tDelete = 60 tDelete = 60
adsbAssociator = AdsbAssociator() adsbAssociator = AdsbAssociator()
ellipseParametric = EllipseParametric() ellipseParametric = EllipseParametric("mean", 200, 500)
ellipsoidParametric = EllipsoidParametric() ellipsoidParametric = EllipsoidParametric("mean", 100, 500)
sphericalIntersection = SphericalIntersection() sphericalIntersection = SphericalIntersection()
adsbTruth = AdsbTruth(5) adsbTruth = AdsbTruth(5)
save = True save = True