From 9f31e3190c693d733f9fd7fb469a9152b9146e45 Mon Sep 17 00:00:00 2001 From: 30hours Date: Wed, 13 Mar 2024 13:04:34 +0000 Subject: [PATCH] Fixed ellipse and ellipsoid methoids --- event/algorithm/geometry/Geometry.py | 5 +- .../localisation/EllipseParametric.py | 130 +++++++----------- .../localisation/EllipsoidParametric.py | 70 +++++++--- .../localisation/SphericalIntersection.py | 3 +- event/event.py | 4 +- 5 files changed, 112 insertions(+), 100 deletions(-) diff --git a/event/algorithm/geometry/Geometry.py b/event/algorithm/geometry/Geometry.py index a935c5a..b6284b8 100644 --- a/event/algorithm/geometry/Geometry.py +++ b/event/algorithm/geometry/Geometry.py @@ -189,4 +189,7 @@ class Geometry: return math.sqrt( (point2[0]-point1[0])**2 + (point2[1]-point1[1])**2 + - (point2[2]-point1[2])**2) \ No newline at end of file + (point2[2]-point1[2])**2) + + def average_points(points): + return [sum(coord) / len(coord) for coord in zip(*points)] \ No newline at end of file diff --git a/event/algorithm/localisation/EllipseParametric.py b/event/algorithm/localisation/EllipseParametric.py index b912897..ce93ef5 100644 --- a/event/algorithm/localisation/EllipseParametric.py +++ b/event/algorithm/localisation/EllipseParametric.py @@ -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) + + 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: + 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"] = [] @@ -190,37 +193,4 @@ class EllipseParametric: output.append([x, y, z]) 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)] \ No newline at end of file + \ No newline at end of file diff --git a/event/algorithm/localisation/EllipsoidParametric.py b/event/algorithm/localisation/EllipsoidParametric.py index 5057701..438c5f6 100644 --- a/event/algorithm/localisation/EllipsoidParametric.py +++ b/event/algorithm/localisation/EllipsoidParametric.py @@ -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] = {} @@ -162,4 +200,4 @@ class EllipsoidParametric: [x, y, z] = Geometry.lla2ecef(x, y, z) output.append([round(x, 3), round(y, 3), round(z)]) - return output \ No newline at end of file + return output diff --git a/event/algorithm/localisation/SphericalIntersection.py b/event/algorithm/localisation/SphericalIntersection.py index 6b8fae7..6cd5589 100644 --- a/event/algorithm/localisation/SphericalIntersection.py +++ b/event/algorithm/localisation/SphericalIntersection.py @@ -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] diff --git a/event/event.py b/event/event.py index 4804ffe..17df55d 100644 --- a/event/event.py +++ b/event/event.py @@ -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