Last active
October 16, 2017 18:36
-
-
Save dkbarn/abc54c481006f879b4d020dd94f54ac9 to your computer and use it in GitHub Desktop.
Trilateration of n gps regions
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import itertools | |
import math | |
class PointCluster(object): | |
def __init__(self, pts=()): | |
self.pts = [] | |
self.pts.extend(pts) | |
@property | |
def centroid(self): | |
return ( | |
sum([pt[0] for pt in self.pts]) / len(self.pts), | |
sum([pt[1] for pt in self.pts]) / len(self.pts) | |
) | |
def trilaterate_gps_regions(gps_regions, merge_dist=0): | |
""" | |
Given a set of GpsRegions (lat,long,dist), perform trilateration and find all GpsPoints | |
representing the intersection points between the regions, where at least 3 regions must be contributing | |
toward each point. The merge_dist argument defines the distance between region boundaries and points | |
at which they will be considered part of the same location and clustered together. | |
Returns a map of {GpsPoint: [list of GpsRegions intersecting at this point]} | |
""" | |
# first, map from unique x,y,r coords to GpsRegions | |
circle_to_region = {} # { (x,y,r): [list of GpsRegions]} | |
for gps_region in gps_regions: | |
# convert gps regions into x,y,r circles | |
# https://stackoverflow.com/questions/16266809/convert-from-latitude-longitude-to-x-y | |
circle_to_region.setdefault((x,y,r), []).append(gps_region) | |
# iterate over every possible combination of circles | |
# building a list of all intersection points or near-intersection points | |
isectpt_to_circle = {} # { intersect_pt: set([unique x,y,r values]) } | |
for circle1, circle2 in itertools.combinations(circle_to_region, 2): | |
(x1,y1,r1) = circle1 | |
(x2,y2,r2) = circle2 | |
# calculate distance between the circles | |
dx = x2-x1 | |
dy = y2-y1 | |
d = math.sqrt(dx**2 + dy**2) | |
if math.abs(r2-r1) <= d <= r1+r2: | |
# the two circles intersect at one or two points | |
# https://stackoverflow.com/questions/3349125/circle-circle-intersection-points | |
a = (r1**2 - r2**2 + d**2) / (2*d) | |
h = math.sqrt(r1**2 - a**2) | |
xm = x1 + a*dx/d | |
ym = y1 + a*dy/d | |
pt1 = (xm + h*dy/d, ym - h*dx/d) | |
pt2 = (xm - h*dy/d, ym + h*dx/d) | |
# add intersection points to the map | |
isectpt_to_circle.setdefault(pt1, set()).update([circle1, circle2]) | |
isectpt_to_circle.setdefault(pt2, set()).update([circle1, circle2]) | |
else: | |
# the circles do not intersect | |
# they are either disjoint or one completely contains the other | |
# calculate the midpoint between the two circles | |
if d > r1+r2: | |
dist_along_line = 0.5*(d+r1-r2) | |
else: | |
dist_along_line = 0.5*(d+r1+r2) | |
# compute the line connecting their centers | |
m = (y2-y1) / (x2-x1) | |
x = x1 + dist_along_line/math.sqrt(1 + m**2) | |
y = m*x + y1 | |
# only accept if the midpoint is within merge distance of the circles | |
dist = math.sqrt((x-x1)**2 + (y-y1)**2) | |
if dist <= merge_dist: | |
isectpt_to_circle.setdefault((x,y), set()).update([circle1, circle2]) | |
# now it's simply a clustering problem | |
# begin with a single cluster at each intersection point | |
clusters = set([PointCluster([pt]) for pt in isectpt_to_circle]) | |
# repeatedly merge together the closest two intersection points within mergeable distance | |
while True: | |
min_dist = sys.maxint | |
min_clusters = () | |
for cluster1, cluster2 in itertools.combinations(clusters, 2): | |
(x1,y1) = cluster1.centroid | |
(x2,y2) = cluster2.centroid | |
d = math.sqrt((x2-x1)**2 + (y2-y1)**2) | |
if d < min_dist: | |
min_dist = d | |
min_clusters = (cluster1, cluster2) | |
if min_dist <= merge_dist: | |
# merge the two clusters together, and repeat | |
(cluster1, cluster2) = min_clusters | |
cluster1.pts.extend(cluster2.pts) | |
clusters.discard(cluster2) | |
else: | |
# there are no remaining clusters within merge distance, stop here | |
break | |
# return gps points for each of the clusters containing at least 3 unique circles (minimum required for trilateration) | |
# mapped to the corresponding gps regions intersecting at that point | |
gps_points = {} | |
for cluster in clusters: | |
circles = set([isectpt_to_circle[pt] for pt in cluster.pts]) | |
if len(circles) >= 3: | |
# convert cluster centroid back into gps coordinates | |
cluster.centroid | |
gps_regions = itertools.chain(*[circle_to_region[c] for c in circles]) | |
gps_points[gps_point] = gps_regions | |
return gps_points |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment