bisection.py 2.67 KB
Newer Older
tilman's avatar
tilman committed
1
2
import numpy as np
import numpy.linalg as la
tilman's avatar
tilman committed
3
4
5
6
7
8
9
10
from shapely.geometry import Polygon
import itertools

def polyToArr(poly):
    if poly is not None and poly.exterior is not None:
        return np.array(poly.exterior.coords.xy).transpose().astype(int)
    else:
        return None
tilman's avatar
tilman committed
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

def getAngle(a,b,c):
    ba = a - b
    bc = c - b
    cosine_angle = np.dot(ba, bc) / (np.linalg.norm(ba) * np.linalg.norm(bc))
    angle = np.arccos(cosine_angle)
    return angle

def getBisecPoint(a,b,c):
    angle = getAngle(a,b,c)
    if(a[0]-b[0]<0): #check wich direction (left/right) the vector should point
        angle = 360 - angle
    else:
        angle = 180 - angle
    angle = angle / 2
    dist = la.norm(a-b)
    # dist = 100
    d = (int(dist * np.cos(angle)), int(dist * np.sin(angle))) #with origin zero
    return (d[0]+b[0],d[1]+b[1]) #with origin b

tilman's avatar
tilman committed
31
32
33
34
35
36
37
38
39
40
41
42
43
44
def getBisecCone(a,b,c,width):
    angle = getAngle(a,b,c)
    if(a[0]-b[0]<0): #check wich direction (left/right) the vector should point
        angle = 360 - angle
    else:
        angle = 180 - angle
    angle = angle / 2
    #dist = la.norm(a-b)
    dist = 1500
    # dist = 100
    conePoint1 = (int(dist * np.cos(angle-(width/2))), int(dist * np.sin(angle-(width/2)))) #with origin zero
    conePoint2 = (int(dist * np.cos(angle+(width/2))), int(dist * np.sin(angle+(width/2)))) #with origin zero
    return ((conePoint1[0]+b[0],conePoint1[1]+b[1]),(conePoint2[0]+b[0],conePoint2[1]+b[1]))

tilman's avatar
tilman committed
45
46
47
48
49
50
51
def poseToBisectVector(pose):
    points = pose[[0,1,8]]
    if(0.0 in points[:,2:]): #if one point has confidence zero, we can not generate the vector
        return None
    a,b,c = points[:,:2] # cut of confidence score so we have normal coordinate points
    bisecPoint = getBisecPoint(a,b,c)
    return np.array([bisecPoint,b])
tilman's avatar
tilman committed
52
53
54
55
56
57
58
59
60
61
62
    #return Polygon([bisecPoint,b])

def poseToBisectCone(pose, angle):
    width = np.deg2rad(angle)
    points = pose[[0,1,8]]
    if(0.0 in points[:,2:]): #if one point has confidence zero, we can not generate the vector
        return None
    a,b,c = points[:,:2] # cut of confidence score so we have normal coordinate points
    conePoint1, conePoint2 = getBisecCone(a,b,c,width)
    #return np.array([conePoint1, conePoint2, b])
    return Polygon([conePoint1, conePoint2, b])
tilman's avatar
tilman committed
63

tilman's avatar
tilman committed
64
65
def coneIntersections(bisecCones):
    out = {}
tilman's avatar
tilman committed
66
    for r in range(1,len(bisecCones)):
tilman's avatar
tilman committed
67
68
69
70
71
72
73
74
75
76
        pc = list(itertools.combinations(range(0,len(bisecCones)),r))
        #print(pc)
        for combi in pc:
            # print(combi)
            intersect = bisecCones[combi[0]]
            for i in combi[1:]:
                intersect = intersect.intersection(bisecCones[i])
            if not intersect.is_empty:
                out[combi] = intersect
    return out