mirror of
https://github.com/30hours/3lips.git
synced 2024-11-18 12:33:58 +00:00
Fixed ellipse and ellipsoid methoids
This commit is contained in:
30hours
parent
21c2d549f3
commit
9f31e3190c
5 changed files with 112 additions and 100 deletions
|
|
@ -189,4 +189,7 @@ class Geometry:
|
|||
return math.sqrt(
|
||||
(point2[0]-point1[0])**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)]
|
||||
|
|
@ -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)]
|
||||
|
||||
|
|
@ -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
|
||||
return output
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in a new issue