bisection.py 4.06 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): #checked is correct
tilman's avatar
tilman committed
13
14
15
16
17
    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
tilman's avatar
tilman committed
18
def getAngleGroundNormed(a,b,c, CORRECTION_ANGLE):
19
20
21
22
    if(a[0]-b[0]<0):
        b_plane_point = np.array([b[0]-50,b[1]])
    else:
        b_plane_point = np.array([b[0]+50,b[1]])
tilman's avatar
tilman committed
23
    vector_angle = getAngle(a,b,c, CORRECTION_ANGLE)
24
    ground_angle = getAngle(a,b,b_plane_point, CORRECTION_ANGLE)
tilman's avatar
tilman committed
25
    normed_angle = vector_angle/2 - ground_angle
26
27
28
29
    # print("points",a,b,c,b_plane_point)
    # print("angles",np.rad2deg(vector_angle),np.rad2deg(ground_angle),np.rad2deg(normed_angle))

    if(a[0]-b[0]<0):
30
        # print("normed_angle l",np.rad2deg(normed_angle),np.rad2deg((normed_angle+np.deg2rad(180-CORRECTION_ANGLE))))
31
        return (normed_angle+np.deg2rad(180-CORRECTION_ANGLE))
32
    else:
33
        # print("normed_angle r",np.rad2deg(normed_angle),np.rad2deg((np.deg2rad(360+CORRECTION_ANGLE)-normed_angle)))
34
        return (np.deg2rad(360+CORRECTION_ANGLE)-normed_angle)
tilman's avatar
tilman committed
35

36
37
38
39
40
41
42
43
44
45
def angleMapper(pose, CORRECTION_ANGLE):
    angle = getAngleGroundNormed(*pose[[0,1,8]][:,:2], CORRECTION_ANGLE)
     #map all angles to one direction, so the mean does not get influences by left/right direction
    if(angle > np.deg2rad(180)):
        mapped = angle - np.deg2rad(180)
        return mapped - np.deg2rad(180) if mapped > np.deg2rad(90) else mapped
    else:
        mapped = angle
        return mapped - np.deg2rad(180) if mapped > np.deg2rad(90) else mapped

tilman's avatar
tilman committed
46
def getGlobalLineAngle(poses, CORRECTION_ANGLE):
47
    return np.mean([angleMapper(pose, CORRECTION_ANGLE) for pose in poses if not 0.0 in pose[[0,1,8]][:,2:]])
tilman's avatar
tilman committed
48
49

def getBisecPoint(a,b,c, CORRECTION_ANGLE):
tilman's avatar
tilman committed
50
    angle = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE)
51
    dist = la.norm(a-b)*2
tilman's avatar
tilman committed
52
    d = (int(dist * np.cos(angle)), int(dist * np.sin(angle))) #with origin zero
tilman's avatar
tilman committed
53
54
    out = (b[0]+d[0],b[1]-d[1])
    return out #with origin b
tilman's avatar
tilman committed
55

56
def getBisecCone(a,b,c, length, BISEC_CONE_ANGLE, CORRECTION_ANGLE):
57
    angle = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE)
58
59
    conePoint1 = (int(length * np.cos(angle-(BISEC_CONE_ANGLE/2))), int(length * np.sin(angle-(BISEC_CONE_ANGLE/2)))) #with origin zero
    conePoint2 = (int(length * np.cos(angle+(BISEC_CONE_ANGLE/2))), int(length * np.sin(angle+(BISEC_CONE_ANGLE/2)))) #with origin zero
60
    return ((b[0]+conePoint1[0],b[1]-conePoint1[1]),(b[0]+conePoint2[0],b[1]-conePoint2[1]))
tilman's avatar
tilman committed
61

tilman's avatar
tilman committed
62
def poseToBisectVector(pose, CORRECTION_ANGLE):
tilman's avatar
tilman committed
63
64
65
66
    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
67
    bisecPoint = getBisecPoint(a,b,c, CORRECTION_ANGLE)
tilman's avatar
tilman committed
68
    return np.array([bisecPoint,b])
tilman's avatar
tilman committed
69
70
    #return Polygon([bisecPoint,b])

71
def poseToBisectCone(pose, length, angle, CORRECTION_ANGLE):
tilman's avatar
tilman committed
72
73
74
75
76
    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
77
    conePoint1, conePoint2 = getBisecCone(a,b,c,length,width, CORRECTION_ANGLE)
tilman's avatar
tilman committed
78
79
    #return np.array([conePoint1, conePoint2, b])
    return Polygon([conePoint1, conePoint2, b])
tilman's avatar
tilman committed
80

tilman's avatar
tilman committed
81
82
def coneIntersections(bisecCones):
    out = {}
tilman's avatar
tilman committed
83
    for r in range(1,len(bisecCones)+1):
tilman's avatar
tilman committed
84
85
86
87
88
89
90
91
92
93
        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