add demo keypoints and body line for final composition

eb193ec3 · tilman · 089641fd · eb193ec3 · eb193ec3 · eb193ec3
Unverified Commit eb193ec3 authored Dec 26, 2019 by tilman
--- a/detect_structures.py
+++ b/detect_structures.py
@@ -5,6 +5,7 @@ import cv2
 import numpy as np
 import os
 from sys import platform
+from dotmap import DotMap
 #sys.path.append('/Users/Tilman/Documents/Programme/Python/forschungspraktikum/openpose/python');
 from openpose import pyopenpose as op
 from lib.triangles import *
@@ -12,19 +13,30 @@ from lib.bisection import *
 from lib.misc import *

 SHOW_WINDOW = True
-SAVE_FILE = False
-TRIANGLES = True
+SAVE_FILE = not SHOW_WINDOW
+TRIANGLES = False
+BODY_LINES = True
 BISEC_VECTORS = True
 BISEC_CONES = True
 GLOBAL_LINE = True
-BISEC_CONE_ANGLE = 40
-CORRECTION_ANGLE = 30
+BISEC_CONE_ANGLE = 50
+CORRECTION_ANGLE = 23
 OVERLAY_ALPHA = 0.2
-BISEC_SKIP_LOWER_LEVELS = False
-OUT_DIR = 'images/out/images_imdahl/main_structure_cones'
+BISEC_SKIP_LOWER_LEVELS = True
+DISPLAY_RASTER_ELEMENTS = 500
+OUT_DIR = 'images/out/images_imdahl/main_structure_global_line'
 #IN_DIR = "images/first_email/"     # images from first email
 IN_DIR = "images/images_imdahl/"    # images from imdahl

+SKIP_OPENPOSE = False
+OPENPOSE_DEMO_KEYPOINTS = np.array([[[4.7613028e+02, 3.3695804e+02, 9.0203685e-01],[5.3667474e+02, 3.8633786e+02, 6.6615295e-01],[5.1645105e+02, 3.8405157e+02, 5.1514143e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[5.5459924e+02, 3.8859457e+02, 6.4240879e-01],[5.6353766e+02, 4.7384988e+02, 1.8810490e-01],[5.3886292e+02, 5.2543573e+02, 9.0144195e-02],[5.4566248e+02, 5.3215259e+02, 3.6083767e-01],[5.2768524e+02, 5.3213129e+02, 3.1196830e-01],[5.4556714e+02, 6.3534674e+02, 1.8182488e-01],[5.8149310e+02, 7.2958716e+02, 1.3625422e-01],[5.6579541e+02, 5.3216382e+02, 3.6866242e-01],[5.8822272e+02, 6.2862476e+02, 1.7708556e-01],[6.0843213e+02, 7.2955762e+02, 2.2736737e-01],[4.7597812e+02, 3.2798129e+02, 5.7176876e-01],[4.8729745e+02, 3.3027243e+02, 9.1296065e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[5.2090784e+02, 3.3472034e+02, 7.7942842e-01],[5.7928674e+02, 7.5646222e+02, 2.0351715e-01],[5.9049512e+02, 7.5648248e+02, 2.0819387e-01],[6.2183606e+02, 7.3853394e+02, 1.7312977e-01],[5.8145673e+02, 7.5420642e+02, 1.2660497e-01],[5.7701074e+02, 7.5417773e+02, 1.2881383e-01],[5.8374255e+02, 7.3627380e+02, 9.4869599e-02]]
+                                    ,[[6.4435681e+02, 3.6383255e+02, 8.9096022e-01],[6.6903070e+02, 3.9760306e+02, 8.7681645e-01],[6.4430103e+02, 3.9525812e+02, 7.9584122e-01],[6.3310535e+02, 4.5589160e+02, 3.7108111e-01],[5.9046979e+02, 4.2451276e+02, 4.0277350e-01],[6.9366602e+02, 4.0197583e+02, 8.9528430e-01],[6.8247137e+02, 4.6042902e+02, 5.5132395e-01],[6.0616620e+02, 4.3569894e+02, 3.4303352e-01],[6.5551196e+02, 5.1196445e+02, 2.9572365e-01],[6.3529651e+02, 5.0747903e+02, 2.8629595e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[6.7573169e+02, 5.1421967e+02, 3.0180413e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[6.4206000e+02, 3.5276721e+02, 7.2430253e-01],[6.5327673e+02, 3.5271103e+02, 9.4265050e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[6.7577380e+02, 3.5269864e+02, 8.9672232e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]
+                                    ,[[7.2723553e+02, 4.0875150e+02, 8.3982950e-01],[7.6091986e+02, 4.6032086e+02, 5.0676465e-01],[7.3178253e+02, 4.5359366e+02, 3.5797939e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.8784674e+02, 4.6483188e+02, 5.6356871e-01],[7.6320721e+02, 5.6802844e+02, 3.7939239e-01],[7.2953772e+02, 5.4564911e+02, 1.5424372e-01],[7.6546356e+02, 6.1964557e+02, 1.7308682e-01],[7.3854327e+02, 6.1513757e+02, 1.5351829e-01],[7.3855487e+02, 7.3405249e+02, 5.6986582e-02],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.8789227e+02, 6.2636108e+02, 1.8666090e-01],[7.9010718e+02, 7.5197815e+02, 9.0752751e-02],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.2722571e+02, 3.9980579e+02, 4.9854943e-01],[7.4074554e+02, 4.0420221e+02, 8.2562774e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.6537799e+02, 4.0880304e+02, 6.8228495e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]
+                                    ,[[2.6297342e+02, 3.4823679e+02, 9.1535652e-01],[2.1584425e+02, 3.8410617e+02, 4.2777365e-01],[2.0466562e+02, 3.8629623e+02, 6.5148002e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[2.2483388e+02, 3.7963403e+02, 2.8349286e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[2.1584836e+02, 5.5681036e+02, 7.1318626e-02],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[2.5625162e+02, 3.3922253e+02, 8.9375269e-01],[2.6528430e+02, 3.3701016e+02, 1.3707811e-01],[2.2490630e+02, 3.4151849e+02, 8.1041366e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]
+                                    ,[[3.1685654e+02, 3.3244104e+02, 6.7855740e-01],[2.9669766e+02, 3.7735825e+02, 3.5962355e-01],[2.6300262e+02, 3.8186972e+02, 4.8755571e-01],[2.8984323e+02, 5.1421375e+02, 1.4892229e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[3.3024896e+02, 3.7068640e+02, 3.1298172e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[3.0774149e+02, 3.2796570e+02, 6.2570477e-01],[3.1678952e+02, 3.1911349e+02, 2.6238269e-01],[2.8093988e+02, 3.3702823e+02, 4.3097427e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]
+                                    ,[[4.0661322e+02, 3.3243243e+02, 7.2449613e-01],[3.5496320e+02, 3.7965060e+02, 3.2941282e-01],[2.0466562e+02, 3.8629623e+02, 6.5148002e-01],[2.4725473e+02, 4.3794165e+02, 1.0388593e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[3.9527917e+02, 3.7732455e+02, 1.9104436e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[3.9753812e+02, 3.2572601e+02, 7.9601538e-01],[4.1098145e+02, 3.2347913e+02, 4.7544584e-01],[3.5937631e+02, 3.2570648e+02, 6.8124008e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]
+                                    ,[[1.1046178e+02, 3.3481174e+02, 8.0748719e-01],[5.2089359e+01, 3.7064417e+02, 1.8357244e-01],[6.7774979e+01, 3.6842288e+02, 4.0538907e-01],[1.3283961e+02, 3.8408841e+02, 2.2997330e-01],[1.7771373e+02, 3.4149902e+02, 2.7701011e-01],[3.4127533e+01, 3.6390732e+02, 1.1019738e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[9.9306007e+01, 3.3019724e+02, 9.2014235e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.8990585e+01, 3.3474988e+02, 8.4556317e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]
+                                    ,[[7.0929346e+02, 3.5261667e+02, 3.9232758e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.8120416e+02, 4.0420923e+02, 5.1513046e-02],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.0712549e+02, 3.4816431e+02, 1.5942883e-01],[7.1832990e+02, 3.4808749e+02, 3.8954309e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[7.4299701e+02, 3.5270523e+02, 2.7498546e-01],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00],[0.0000000e+00, 0.0000000e+00, 0.0000000e+00]]])

 ## misc functions
 def overlay_two_image_v2(image, overlay, ignore_color=[0,0,0], alpha=0.1):
@@ -40,10 +52,10 @@ params = dict()
 params["model_folder"] = os.environ['OPENPOSE_MODELS']

 # Starting OpenPose
-opWrapper = op.WrapperPython()
-opWrapper.configure(params)
-opWrapper.start()
-
+if not SKIP_OPENPOSE:
+    opWrapper = op.WrapperPython()
+    opWrapper.configure(params)
+    opWrapper.start()

 images = [os.path.join(os.getcwd(), IN_DIR, f) for f in os.listdir(IN_DIR)] #make path absolute so os.chdir has no side effects

@@ -51,28 +63,21 @@ os.chdir(OUT_DIR) #save images in this dir
 for img_name in images:
    # Process Image
    print("calculating: "+img_name)
-    datum = op.Datum()
    img = cv2.imread(img_name)
+    max_lw = max(len(img),len(img[0]))
+    esz = max_lw / DISPLAY_RASTER_ELEMENTS
+    if SKIP_OPENPOSE:
+        datum = DotMap()
+        datum.poseKeypoints = OPENPOSE_DEMO_KEYPOINTS
+        print("Skipping OPENPOSE")
+    else:
+        datum = op.Datum()
+        datum.cvInputData = img
+        opWrapper.emplaceAndPop([datum])
+        # img = datum.cvOutputData

-    datum.cvInputData = img
-    opWrapper.emplaceAndPop([datum])
-
-    img = datum.cvOutputData
-    #overlay = img.copy()
-
-    # print("poses:"+str(datum.poseKeypoints))
-    if TRIANGLES:
-        triangles = [poseToTriangle(pose) for pose in datum.poseKeypoints]
-        for triangle in triangles:
-            if triangle is not None:
-                cv2.drawContours(img, [triangle], 0, 255, -1)
-    if BISEC_VECTORS:
-        bisecVectors = [poseToBisectVector(pose, CORRECTION_ANGLE) for pose in datum.poseKeypoints]
-        for bisecVector in bisecVectors:
-            if bisecVector is not None:
-                cv2.arrowedLine(img, trp(bisecVector[1]), trp(bisecVector[0]), (0,0,255), 2)
-    if BISEC_CONES:
-        bisecCones = [poseToBisectCone(pose, BISEC_CONE_ANGLE, CORRECTION_ANGLE) for pose in datum.poseKeypoints]
+    if BISEC_CONES or GLOBAL_LINE:
+        bisecCones = [poseToBisectCone(pose, max_lw, BISEC_CONE_ANGLE, CORRECTION_ANGLE) for pose in datum.poseKeypoints] #use max(img.height,width) as cone length
        bisecCones = [v for v in bisecCones if v] #remove None values
        #print([polyToArr(bisecCone) for bisecCone in bisecCones])
        intersections = coneIntersections(bisecCones)
@@ -84,16 +89,36 @@ for img_name in images:
            else:
                overlay = np.zeros((len(img),len(img[0]),3), np.uint8)
                color = min(((len(combi)-1)*150,255))
-                cv2.drawContours(overlay, [polyToArr(intersections[combi])], 0, (color,0,(0 if is_not_last_level else 255)), -1)
+                if BISEC_CONES:
+                    cv2.drawContours(overlay, [polyToArr(intersections[combi])], 0, (color,0,(0 if is_not_last_level else 255)), -1)
+                img = overlay_two_image_v2(img, overlay, [0,0,0], (OVERLAY_ALPHA if is_not_last_level else 0.6))
                if not is_not_last_level and GLOBAL_LINE: #draw centroid of last polygon
                    xy = (int(intersections[combi].centroid.x),int(intersections[combi].centroid.y))
-                    cv2.circle(overlay, xy, 10, (0,255,0), -1)
                    global_angle = getGlobalLineAngle(datum.poseKeypoints, CORRECTION_ANGLE)
-                    print("global_angle",global_angle)
-                    dist = 1000
+                    print("global_angle",np.rad2deg(global_angle))
+                    dist = max_lw
                    d = (int(dist * np.cos(global_angle)), int(dist * np.sin(global_angle))) #with origin zero
-                    cv2.line(overlay, xy, (xy[0]+d[0],xy[1]+d[1]), (0,0,255), 2)
-                img = overlay_two_image_v2(img, overlay, [0,0,0], (OVERLAY_ALPHA if is_not_last_level else 0.9))
+                    d_l = (int(-dist * np.cos(global_angle)), int(-dist * np.sin(global_angle))) #with origin zero
+                    # draw line with global gaze angle (special mean of all gaze angles) and through center of last intersection
+                    cv2.line(img, xy, (xy[0]+d[0],xy[1]-d[1]), (0,255,255), int(10*esz))
+                    cv2.line(img, xy, (xy[0]+d_l[0],xy[1]-d_l[1]), (0,255,255), int(10*esz))
+                    cv2.circle(img, xy, int(10*esz), (255,255,0), -1)
+
+    if TRIANGLES or BODY_LINES:
+        triangles = [poseToTriangle(pose) for pose in datum.poseKeypoints]
+        for triangle in triangles:
+            if triangle is not None:
+                if TRIANGLES:
+                    cv2.drawContours(img, [triangle], 0, 255, -1)
+                if BODY_LINES:
+                    linePoints = triangleToBodyLine(triangle)
+                    cv2.line(img, trp(linePoints[0]), trp(linePoints[1]), (0,255,0), int(6*esz))
+
+    if BISEC_VECTORS:
+        bisecVectors = [poseToBisectVector(pose, CORRECTION_ANGLE) for pose in datum.poseKeypoints]
+        for bisecVector in bisecVectors:
+            if bisecVector is not None:
+                cv2.arrowedLine(img, trp(bisecVector[1]), trp(bisecVector[0]), (0,0,255), int(4*esz))

    if SAVE_FILE:
        cv2.imwrite(os.path.basename(img_name),img)
@@ -102,5 +127,5 @@ for img_name in images:
        cv2.imshow(img_name, img)
        cv2.waitKey(0)
 if SHOW_WINDOW:
-    cv2.waitKey(0)  
+    cv2.waitKey(0)
    cv2.destroyAllWindows()
\ No newline at end of file
--- a/lib/bisection.py
+++ b/lib/bisection.py
@@ -27,12 +27,24 @@ def getAngleGroundNormed(a,b,c, CORRECTION_ANGLE):
    # print("angles",np.rad2deg(vector_angle),np.rad2deg(ground_angle),np.rad2deg(normed_angle))

    if(a[0]-b[0]<0):
+        # print("normed_angle l",np.rad2deg(normed_angle),np.rad2deg((normed_angle+np.deg2rad(180-CORRECTION_ANGLE))))
        return (normed_angle+np.deg2rad(180-CORRECTION_ANGLE))
    else:
+        # print("normed_angle r",np.rad2deg(normed_angle),np.rad2deg((np.deg2rad(360+CORRECTION_ANGLE)-normed_angle)))
        return (np.deg2rad(360+CORRECTION_ANGLE)-normed_angle)

+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
+
 def getGlobalLineAngle(poses, CORRECTION_ANGLE):
-    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:]])
+    return np.mean([angleMapper(pose, 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)
@@ -41,11 +53,10 @@ def getBisecPoint(a,b,c, CORRECTION_ANGLE):
    out = (b[0]+d[0],b[1]-d[1])
    return out #with origin b

-def getBisecCone(a,b,c,width, CORRECTION_ANGLE):
+def getBisecCone(a,b,c, length, width, CORRECTION_ANGLE):
    angle = getAngleGroundNormed(a,b,c, CORRECTION_ANGLE)
-    dist = 1500
-    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
+    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
    return ((b[0]+conePoint1[0],b[1]-conePoint1[1]),(b[0]+conePoint2[0],b[1]-conePoint2[1]))

 def poseToBisectVector(pose, CORRECTION_ANGLE):
@@ -57,13 +68,13 @@ def poseToBisectVector(pose, CORRECTION_ANGLE):
    return np.array([bisecPoint,b])
    #return Polygon([bisecPoint,b])

-def poseToBisectCone(pose, angle, CORRECTION_ANGLE):
+def poseToBisectCone(pose, length, angle, CORRECTION_ANGLE):
    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
-    conePoint1, conePoint2 = getBisecCone(a,b,c,width, CORRECTION_ANGLE)
+    conePoint1, conePoint2 = getBisecCone(a,b,c,length,width, CORRECTION_ANGLE)
    #return np.array([conePoint1, conePoint2, b])
    return Polygon([conePoint1, conePoint2, b])


--- a/lib/triangles.py
+++ b/lib/triangles.py
@@ -27,4 +27,8 @@ def poseToTriangle(pose):
        retval = np.array([(top[0],top[1]),(left[0],left[1]),(right[0],right[1])])
        return np.array(retval).astype(int)
    else:
-        return None
\ No newline at end of file
+        return None
+
+def triangleToBodyLine(triangle):
+    bottom_middle_point = (triangle[1]+triangle[2])/2
+    return (triangle[0],bottom_middle_point)