Fixed ellipse and ellipsoid methoids

event/algorithm/geometry/Geometry.py

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

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.nSamples = nSamples
-        self.threshold  = 800
+        self.threshold  = threshold
+        self.method = method
 
     def process(self, assoc_detections, radar_data):
 
@@ -87,24 +88,29 @@ class EllipseParametric:
             radar_keys = list(target_samples[target].keys())
             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)
@@ -128,12 +134,9 @@ class EllipseParametric:
                 else:
                     return output
 
-            # find closest points bruteforce
+            else:
-            # points = list(target_samples[target].values())
+                print('Invalid method.')
-            # result_points, result_distance = self.closest_points_bruteforce(points)
+                return output
-            # 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] = {}
@@ -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)]

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.nSamples = nSamples
-        self.threshold  = 800
+        self.threshold  = threshold
+        self.method = method
 
     def process(self, assoc_detections, radar_data):
 
@@ -84,7 +85,9 @@ class EllipsoidParametric:
             radar_keys = list(target_samples[target].keys())
             samples_intersect = []
 
-            # loop points in master ellipsoid
+            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
@@ -97,6 +100,41 @@ class EllipsoidParametric:
                     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] = {}
             output[target]["points"] = []

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]

event/event.py

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