getBisecPoint and getAngleGroundNormed working

bisectVectorTest.py

@@ -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)

detect_structures.py

@@ -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:

lib/bisection.py

@@ -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))
+    # 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))
-    #return (normed_angle)
-    # ground_angle = getAngle(a,b,c, CORRECTION_ANGLE)
+    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