bisection.py 3.19 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

tilman's avatar
tilman committed
12
def getAngle(a,b,c, CORRECTION_ANGLE):
tilman's avatar
tilman committed
13
14
15
16
    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)
tilman's avatar
tilman committed
17
18
19
20
21
22
23
24
25
    print("angle",angle,np.rad2deg(angle),360-np.rad2deg(angle),180-np.rad2deg(angle)) 
    # TODO get the angle right....
    # angle = angle - np.deg2rad(CORRECTION_ANGLE)
    if(a[0]-b[0]<0): #check wich direction (left/right) the vector should point
        print("left angle",angle,np.rad2deg(angle))
        angle = 360 - angle #- np.deg2rad(CORRECTION_ANGLE)
    else:
        print("right angle",angle,np.rad2deg(angle))
        angle = 180 - angle #+ np.deg2rad(CORRECTION_ANGLE)
tilman's avatar
tilman committed
26
27
    return angle

tilman's avatar
tilman committed
28
29
30
31
32
def getGlobalLineAngle(poses, CORRECTION_ANGLE):
    return np.mean([getAngle(*pose[[0,1,8]][:,:2], CORRECTION_ANGLE) for pose in poses if not 0.0 in pose[[0,1,8]][:,2:]])

def getBisecPoint(a,b,c, CORRECTION_ANGLE):
    angle = getAngle(a,b,c, CORRECTION_ANGLE)
tilman's avatar
tilman committed
33
34
35
36
37
38
    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
39
40
def getBisecCone(a,b,c,width, CORRECTION_ANGLE):
    angle = getAngle(a,b,c, CORRECTION_ANGLE)
tilman's avatar
tilman committed
41
42
43
44
45
46
47
48
    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
49
def poseToBisectVector(pose, CORRECTION_ANGLE):
tilman's avatar
tilman committed
50
51
52
53
    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
tilman's avatar
tilman committed
54
    bisecPoint = getBisecPoint(a,b,c, CORRECTION_ANGLE)
tilman's avatar
tilman committed
55
    return np.array([bisecPoint,b])
tilman's avatar
tilman committed
56
57
    #return Polygon([bisecPoint,b])

tilman's avatar
tilman committed
58
def poseToBisectCone(pose, angle, CORRECTION_ANGLE):
tilman's avatar
tilman committed
59
60
61
62
63
    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
tilman's avatar
tilman committed
64
    conePoint1, conePoint2 = getBisecCone(a,b,c,width, CORRECTION_ANGLE)
tilman's avatar
tilman committed
65
66
    #return np.array([conePoint1, conePoint2, b])
    return Polygon([conePoint1, conePoint2, b])
tilman's avatar
tilman committed
67

tilman's avatar
tilman committed
68
69
def coneIntersections(bisecCones):
    out = {}
tilman's avatar
tilman committed
70
    for r in range(1,len(bisecCones)+1):
tilman's avatar
tilman committed
71
72
73
74
75
76
77
78
79
80
        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