github-mirrors/3lips
1
0
Fork 0
mirror of https://github.com/30hours/3lips.git synced 2024-11-18 12:33:58 +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

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

@ -190,3 +190,6 @@ class Geometry:
(point2[0]-point1[0])**2 +
(point2[1]-point1[1])**2 +
(point2[2]-point1[2])**2)
def average_points(points):
return [sum(coord) / len(coord) for coord in zip(*points)]

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

@ -20,15 +20,16 @@ class EllipseParametric:
@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.
"""
self.ellipsoids = []
self.nSamples = 150
self.threshold = 800
self.nSamples = nSamples
self.threshold = threshold
self.method = method
def process(self, assoc_detections, radar_data):
@ -87,54 +88,56 @@ class EllipseParametric:
radar_keys = list(target_samples[target].keys())
samples_intersect = []
# loop points in master ellipsoid
# 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)
if self.method == "mean":
# average_point = self.average_points(samples_intersect)
# samples_intersect = [average_point]
# loop points in main ellipsoid
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)
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
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
if min_point1 is not None:
samples_intersect.append(min_point1)
else:
print('Invalid method.')
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
output[target] = {}
output[target]["points"] = []
@ -191,36 +194,3 @@ class EllipseParametric:
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)]

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

@ -17,15 +17,16 @@ class EllipsoidParametric:
@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.
"""
self.ellipsoids = []
self.nSamples = 150
self.threshold = 800
self.nSamples = nSamples
self.threshold = threshold
self.method = method
def process(self, assoc_detections, radar_data):
@ -84,18 +85,55 @@ class EllipsoidParametric:
radar_keys = list(target_samples[target].keys())
samples_intersect = []
# loop points in master ellipsoid
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)
if self.method == "mean":
# loop points in main ellipsoid
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
else:
print('Invalid method.')
return output
# remove duplicates and convert to LLA
output[target] = {}

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

@ -107,11 +107,12 @@ class SphericalIntersection:
a = S_star @ z_vec
b = S_star @ r
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:
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]

4
event/event.py
View file

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