HandofGod/trackingFunctions.py at main · simrunm/HandofGod · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
import cv2
import numpy as np
import math
import time

def get_color_blob(frame, lower_bound, upper_bound, blob_min_size):
    cx, cy, w, h = 0,0,0,0
    hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    mask=cv2.inRange(hsv,lower_bound,upper_bound)
    contours,_= cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    if len(contours) > 0:
        # find largest blob
        max_contour = contours[0]
        for contour in contours:
            if cv2.contourArea(contour)>cv2.contourArea(max_contour):
                max_contour=contour
        contour=max_contour

        if cv2.contourArea(contour) > blob_min_size:
            # get bounding box
            approx=cv2.approxPolyDP(contour, 0.01*cv2.arcLength(contour,True),True)
            x,y,w,h=cv2.boundingRect(approx)
            cv2.rectangle(frame,(x,y),(x+w,y+h),(0,255,0),4)
            # get centroid
            M=cv2.moments(contour)
            cx=int(M['m10']//M['m00'])
            cy=int(M['m01']//M['m00'])
            # cv2.circle(frame, (cx,cy),3,(255,0,0),-1)
            return True, cx, cy, w, h, mask
    return False, cx, cy, w, h, mask

def get_tape_blob(frame, lower_bound, upper_bound, blob_min_size):
    x_vals = []
    y_vals = []
    y_val = 0
    frame_dist = 0
    hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    mask=cv2.inRange(hsv,lower_bound,upper_bound)
    contours,_= cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    if len(contours) > 0:
        # get taep blobs
        tape_blobs = []
        for contour in contours:
            if cv2.contourArea(contour)>blob_min_size:
                tape_blobs.append(contour)

        if len(tape_blobs) == 2:
            for i in range(len(tape_blobs)):
                # get bounding box
                approx=cv2.approxPolyDP(tape_blobs[i], 0.01*cv2.arcLength(tape_blobs[i],True),True)
                x,y,w,h=cv2.boundingRect(approx)
                cv2.rectangle(frame,(x,y),(x+w,y+h),(0,255,0),4)
                # get centroid
                M=cv2.moments(tape_blobs[i])
                cx=int(M['m10']//M['m00'])
                cy=int(M['m01']//M['m00'])
                cv2.circle(frame, (cx,cy),3,(255,0,0),-1)
                x_vals.append(cx)
                y_vals.append(cy)
            y_val = (y_vals[0] + y_vals[1])/2
            frame_dist = x_vals[1] - x_vals[0]
        cv2.imshow("mask_tape",mask)
    return abs(frame_dist), y_val

def finding_theta(x_vert, y_vert,m,b,int_line):
    y_dist = abs(y_vert - int_line)
    x = (y_vert - b)/m
    x_dist = abs(x_vert - x)
    theta = np.arctan(x_dist/y_dist)
    return math.degrees(theta)

def find_x(theta, y, cam_dist):
    # cam_dist is the distance from the start of the side parabola
    # to the edge closer to the board of the top view camera
    # print("theta: ", theta)
    # new change to make cam_dist along the hypotnuse
    # cam_dist = cam_dist / np.cos(theta)

    x = (y + cam_dist)*np.tan(theta)
    return x

def plot_points(lst_points_x, lst_points_y, frame):
        if len(lst_points_x) > 0:
            for i in range(len(lst_points_x)):
                # plot all points on frame
                cv2.circle(frame, (lst_points_x[i], lst_points_y[i]),3,(0,0,255),-1)

def convergence_check(current_prediction, new_prediction, current_time, isprint):
    new_time = time.time()
    rate_of_change = abs(new_prediction-current_prediction)/(new_time-current_time)
    if isprint:
        print("side x real distance: ", new_prediction)
        print("rate_of_change: ", rate_of_change)
    return rate_of_change

# def convergence_check(current_prediction, new_prediction, current_time):
#     new_time = time.time()
#     rate_of_change = abs(new_prediction-current_prediction)/(new_time-current_time)
#     print("rate_of_change: ", rate_of_change)
#     print("side x real distance: ", new_prediction)
#     return rate_of_change

def roc_convergence_check(current_roc, new_roc, current_time):
    new_time = time.time()
    rorochange = abs(new_roc-current_roc)/(new_time-current_time)
    print("rate of rate of change: ", rorochange, end='\n\n')
    return rorochange

def finding_x_int(height, theta):
    if theta < 0 : # going right
        return height * 0.7273 + 249.36
    return height * -0.7273 + 249.36

def find_gradient(height):
    return -0.0044 *  height + 1.4102

def finding_x_start(x_int, x_frame, gradient):
    # x_start = x_int + (x_frame - x_int) * gradient
    x_start = x_frame * 370/640
    return x_start

def get_x_change(predicted_y, theta):
    """
    at 410 gantry y- 9 degrees is the furthest point
    at middle 205 gantry - 11 degrees is the furthest
    at the start 0 gantry - 13 degrees is teh furthest
    """
    # # 0 (predicted_y) -> 13 (angle_range)
    # # 410 (predicted_y) -> 9 (angle_range)
    # angle_range = predicted_y * ((9-13)/410) + 13
    # # -angle_range -> 370
    # # angle_range -> 0
    # x_change = (370*theta)/(2*angle_range)
    # return x_change
    x_change = theta*2
    return x_change