Trilateration of n gps regions

trilateration.py

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