bisection.py 4 KB
 tilman committed Nov 25, 2019 1 2 ``````import numpy as np import numpy.linalg as la `````` tilman committed Dec 08, 2019 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 committed Nov 25, 2019 11 `````` `````` tilman committed Dec 25, 2019 12 ``````def getAngle(a,b,c, CORRECTION_ANGLE): #checked is correct `````` tilman committed Nov 25, 2019 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 committed Dec 25, 2019 18 ``````def getAngleGroundNormed(a,b,c, CORRECTION_ANGLE): `````` tilman committed Dec 25, 2019 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 committed Dec 25, 2019 23 `````` vector_angle = getAngle(a,b,c, CORRECTION_ANGLE) `````` tilman committed Dec 25, 2019 24 `````` ground_angle = getAngle(a,b,b_plane_point, CORRECTION_ANGLE) `````` tilman committed Dec 25, 2019 25 `````` normed_angle = vector_angle/2 - ground_angle `````` tilman committed Dec 25, 2019 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): `````` tilman committed Dec 26, 2019 30 `````` # print("normed_angle l",np.rad2deg(normed_angle),np.rad2deg((normed_angle+np.deg2rad(180-CORRECTION_ANGLE)))) `````` tilman committed Dec 25, 2019 31 `````` return (normed_angle+np.deg2rad(180-CORRECTION_ANGLE)) `````` tilman committed Dec 25, 2019 32 `````` else: `````` tilman committed Dec 26, 2019 33 `````` # print("normed_angle r",np.rad2deg(normed_angle),np.rad2deg((np.deg2rad(360+CORRECTION_ANGLE)-normed_angle))) `````` tilman committed Dec 25, 2019 34 `````` return (np.deg2rad(360+CORRECTION_ANGLE)-normed_angle) `````` tilman committed Nov 25, 2019 35 `````` `````` tilman committed Dec 26, 2019 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 committed Dec 24, 2019 46 ``````def getGlobalLineAngle(poses, CORRECTION_ANGLE): `````` tilman committed Dec 26, 2019 47 `````` return np.mean([angleMapper(pose, CORRECTION_ANGLE) for pose in poses if not 0.0 in pose[[0,1,8]][:,2:]]) `````` tilman committed Dec 24, 2019 48 49 `````` def getBisecPoint(a,b,c, CORRECTION_ANGLE): `````` tilman committed Dec 25, 2019 50 `````` angle = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE) `````` tilman committed Nov 25, 2019 51 52 `````` dist = la.norm(a-b) d = (int(dist * np.cos(angle)), int(dist * np.sin(angle))) #with origin zero `````` tilman committed Dec 25, 2019 53 54 `````` out = (b[0]+d[0],b[1]-d[1]) return out #with origin b `````` tilman committed Nov 25, 2019 55 `````` `````` tilman committed Dec 26, 2019 56 ``````def getBisecCone(a,b,c, length, width, CORRECTION_ANGLE): `````` tilman committed Dec 25, 2019 57 `````` angle = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE) `````` tilman committed Dec 26, 2019 58 59 `````` conePoint1 = (int(length * np.cos(angle-(width/2))), int(length * np.sin(angle-(width/2)))) #with origin zero conePoint2 = (int(length * np.cos(angle+(width/2))), int(length * np.sin(angle+(width/2)))) #with origin zero `````` tilman committed Dec 25, 2019 60 `````` return ((b[0]+conePoint1[0],b[1]-conePoint1[1]),(b[0]+conePoint2[0],b[1]-conePoint2[1])) `````` tilman committed Dec 08, 2019 61 `````` `````` tilman committed Dec 24, 2019 62 ``````def poseToBisectVector(pose, CORRECTION_ANGLE): `````` tilman committed Nov 25, 2019 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 committed Dec 24, 2019 67 `````` bisecPoint = getBisecPoint(a,b,c, CORRECTION_ANGLE) `````` tilman committed Nov 25, 2019 68 `````` return np.array([bisecPoint,b]) `````` tilman committed Dec 08, 2019 69 70 `````` #return Polygon([bisecPoint,b]) `````` tilman committed Dec 26, 2019 71 ``````def poseToBisectCone(pose, length, angle, CORRECTION_ANGLE): `````` tilman committed Dec 08, 2019 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 `````` tilman committed Dec 26, 2019 77 `````` conePoint1, conePoint2 = getBisecCone(a,b,c,length,width, CORRECTION_ANGLE) `````` tilman committed Dec 08, 2019 78 79 `````` #return np.array([conePoint1, conePoint2, b]) return Polygon([conePoint1, conePoint2, b]) `````` tilman committed Nov 25, 2019 80 `````` `````` tilman committed Dec 08, 2019 81 82 ``````def coneIntersections(bisecCones): out = {} `````` tilman committed Dec 24, 2019 83 `````` for r in range(1,len(bisecCones)+1): `````` tilman committed Dec 08, 2019 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``````