Unverified Commit 38b17f7b authored by tilman's avatar tilman
Browse files

getBisecPoint and getAngleGroundNormed working

parent d551b9bf
......@@ -20,7 +20,13 @@ img = cv2.imread(img_name)
bisecVector = poseToBisectVector(pose, CORRECTION_ANGLE)
cv2.arrowedLine(img, trp(bisecVector[1]), trp(bisecVector[0]), (0,0,255), 2)
cv2.arrowedLine(img, trp(pose[1]), trp(pose[0]), (0,255,0), 2)
cv2.arrowedLine(img, (int(pose[1][0]-50),int(pose[1][1])), trp(pose[1]), (255,0,0), 2)
if(pose[0][0]-pose[1][0]<0):
cv2.arrowedLine(img, (int(pose[1][0]-50),int(pose[1][1])), trp(pose[1]), (255,0,0), 2)
else:
cv2.arrowedLine(img, (int(pose[1][0]+50),int(pose[1][1])), trp(pose[1]), (255,0,0), 2)
cv2.arrowedLine(img, trp(pose[1]), trp(pose[8]), (0,255,0), 2)
cv2.namedWindow(img_name, cv2.WINDOW_NORMAL)
......
......@@ -60,7 +60,7 @@ for img_name in images:
img = datum.cvOutputData
#overlay = img.copy()
print("poses:"+str(datum.poseKeypoints))
# print("poses:"+str(datum.poseKeypoints))
if TRIANGLES:
triangles = [poseToTriangle(pose) for pose in datum.poseKeypoints]
for triangle in triangles:
......
......@@ -14,43 +14,35 @@ def getAngle(a,b,c, CORRECTION_ANGLE): #checked is correct
bc = c - b
cosine_angle = np.dot(ba, bc) / (np.linalg.norm(ba) * np.linalg.norm(bc))
angle = np.arccos(cosine_angle)
#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)
return angle
def getAngleGroundNormed(a,b,c, CORRECTION_ANGLE):
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]])
vector_angle = getAngle(a,b,c, CORRECTION_ANGLE)
ground_angle = getAngle(a,b,np.array([b[1]-10,b[1]]), CORRECTION_ANGLE)
ground_angle = getAngle(a,b,b_plane_point, CORRECTION_ANGLE)
normed_angle = vector_angle/2 - ground_angle
print("points",a,b,c,np.array([b[1]-10,b[1]]))
print("angles",np.rad2deg(vector_angle),np.rad2deg(ground_angle),np.rad2deg(normed_angle))
return (normed_angle+np.deg2rad(180))
#return (normed_angle)
# ground_angle = getAngle(a,b,c, CORRECTION_ANGLE)
# 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):
return (normed_angle+np.deg2rad(180))
else:
return (np.deg2rad(360)-normed_angle)
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:]])
return np.mean([getAngleGroundNormed(*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 = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE)
# getAngleGroundNormed(a,b,c, CORRECTION_ANGLE)
# print("getBisecPoint-getAngle full",angle,np.rad2deg(angle))
# angle = angle / 2
print("getBisecPoint-getAngle half",angle,np.rad2deg(angle))
dist = la.norm(a-b)
d = (int(dist * np.cos(angle)), int(dist * np.sin(angle))) #with origin zero
out = (b[0]+d[0],b[1]-d[1])
print("d",d,"b",b,"out",out)
return out #with origin b
def getBisecCone(a,b,c,width, CORRECTION_ANGLE):
angle = getAngle(a,b,c, CORRECTION_ANGLE)
angle = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE)
angle = angle / 2
#dist = la.norm(a-b)
dist = 1500
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment