Flow Chart for Car Tracking

Flow Chart for Car Tracking

Code:

import numpy as np

import cv2

import glob

import covEX

import math

import datetime

# termination criteria

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)

objp = np.zeros((6*8,3), np.float32)

objp[:,:2] = np.mgrid[0:6,0:8].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.

objpoints = [] # 3d point in real world space

imgpoints = [] # 2d points in image plane.

images = glob.glob('./Calibrate_image/*.jpg')

tests = glob.glob('./test_image/*.jpg')

samples = glob.glob('./sample_image/*.jpg')

w = 1920

h = 1080

cap = cv2.VideoCapture(1)

cap.set(3, 1920)

cap.set(4, 1080)

cap.set(cv2.CAP_PROP_AUTOFOCUS, 0)

offset_crop = 15

# Font

thickness_val = 1

font_scale_val = 1.5

text_color = (255,255,255)

font_face = cv2.FONT_HERSHEY_PLAIN

offset = 5

#print("Hi0")

for fname in images:

#while True:

    #print("Hi1")

    img = cv2.imread(fname)

    #(ret1, img) = cap.read()

    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    # Find the chess board corners

    ret, corners = cv2.findChessboardCorners(gray, (6, 8),None)

    #print("Hi2")

    # If found, add object points, image points (after refining them)

    if ret == True:

        #print("Hi3")

        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)

        imgpoints.append(corners2)

        # Draw and display the corners

        #img = cv2.drawChessboardCorners(img, (6,8), corners2,ret)

        #cv2.imshow('img',img)

        #cv2.waitKey(500)

        #while(True):

        

        #    if cv2.waitKey(1) & 0xFF == ord('q'):

        #        break

#print(gray.shape[::-1])

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

color = [(255,0,0),(0,255,0),(0,0,255),(0,0,0)]

i = 0

# q1  q2

# q4  q3

q1 = [554, 95]

q2 = [1235+24, 100]

q3 = [1278+26, 762+25]

q4 = [487, 745+25]

ratio = 1.1

trackH = math.sqrt((q3[0]-q2[0])*(q3[0]-q2[0])+(q3[1]-q2[1])*(q3[1]-q2[1]))

trackW = ratio * trackH

r1 = [q1[0], q1[1]]

r2 = [q1[0]+trackW, q1[1]]

r3 = [q1[0]+trackW, q1[1]+trackH]

r4 = [q1[0], q1[1]+trackH]

pts1 = np.float32([q1, q2, q3, q4])

pts2 = np.float32([r1, r2, r3, r4])

# for fname2 in tests:

    # img = cv2.imread(fname2)

    # [depth, width, height] = img.shape[::-1]

    

    # M = cv2.getPerspectiveTransform(pts1, pts2)

    # img = cv2.warpPerspective(img,M,(width,height)) 

    

    # img_res = img.copy()

    # area_finding = covEX.convertGreenPlant(img)

    

    # im2,contours,hierarchy = cv2.findContours(area_finding,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

    

    # max_x = 0

    # max_y = 0

    # min_x = 999999

    # min_y = 999999

    

    # for cnt in contours:

        # if cv2.contourArea(cnt)>50:

            # [x,y,w,h] = cv2.boundingRect(cnt)

            # print([x,y,w,h])

            # if max_x < x:

                # max_x = x + w

            # elif min_x > x:

                # min_x = x

            # if max_y < y:

                # max_y = y + h

            # elif min_y > y:

                # min_y = y

            # #cv2.rectangle(img,(x,y),(x+w,y+h),color[i],2)

            # #i += 1

            # cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),2)

            

    

    # cv2.rectangle(img,(min_x,min_y),(max_x,max_y),(255,0,0),2)

    # crop_image = img_res[min_y:max_y, min_x:max_x, :]

    # cv2.imshow('Original', img)

    # cv2.imshow('Green Finding', area_finding)

    # cv2.imshow('Crop', crop_image)

    # i = 0

    

    # while True:

        # if cv2.waitKey(1) & 0xFF == ord('q'):

            # break

#print (rvecs)

#print("---------")

#print(tvecs)

# for fname3 in samples:

    # print("---------")

    # crop_image = cv2.imread(fname3)

    

    # max_x = 0

    # max_y = 0

    # min_x = 999999

    # min_y = 999999

    

    # crop_image_res = crop_image.copy()

    # head_finding = covEX.convertRed(crop_image)

    # #head_finding = cv2.GaussianBlur(head_finding,(9,9),0)

    # head_finding = cv2.medianBlur(head_finding,9)

    # tail_finding = covEX.convertGreenPlant(crop_image)

    

    # head_t,head_contours,head_hierarchy = cv2.findContours(head_finding,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

    # tail_t,tail_contours,tail_hierarchy = cv2.findContours(tail_finding,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

    

    # for cnt in head_contours:

        # if cv2.contourArea(cnt)>50:

            # [head_x,head_y,head_w,head_h] = cv2.boundingRect(cnt)

            # print([head_x,head_y,head_w,head_h])

            # cv2.rectangle(crop_image,(head_x,head_y),(head_x+head_w,head_y+head_h),(0,255,0),2)

            

    # for cnt in tail_contours:

        # if cv2.contourArea(cnt)>50:

            # [tail_x,tail_y,tail_w,tail_h] = cv2.boundingRect(cnt)

            # print([tail_x,tail_y,tail_w,tail_h])

            # cv2.rectangle(crop_image,(tail_x,tail_y),(tail_x+tail_w,tail_y+tail_h),(0,255,0),2)

            

    # head_position_x = head_x + (head_w/2)

    # head_position_y = head_y + (head_h/2)

    # #print(head_position_x)

    # #print(head_position_y)

    

    # tail_position_x = tail_x + (tail_w/2)

    # tail_position_y = tail_y + (tail_h/2)

    # #print(tail_position_x)

    # #print(tail_position_y)

    

    # car_position_x = (head_position_x + tail_position_x)/2

    # car_position_y = (head_position_y + tail_position_y)/2

    # print(car_position_x)

    # print(car_position_y)

    

    # car_orientation = math.degrees(math.atan2(head_position_y-tail_position_y, head_position_x-tail_position_x))

    # #if car_orientation < 0:

    # #    car_orientation = math.fabs(car_orientation)

    # #else:

    # #    car_orientation = 360 - car_orientation

    # print(car_orientation)

    

    # car_orientation_text = "Orientation: %.2f" % car_orientation

    # car_position_text = "Car position x: %.2f, Car position y: %.2f" % (car_position_x, car_position_y)

    

    # textHeight = cv2.getTextSize(car_position_text, font_face, font_scale_val, thickness_val)[0][1]

    # cv2.putText(crop_image, car_position_text, (0, textHeight + offset), font_face, font_scale_val, text_color, thickness=thickness_val)

    # cv2.putText(crop_image, car_orientation_text, (0, (textHeight + offset) * 2), font_face, font_scale_val, text_color, thickness=thickness_val)

    

    # cv2.imshow('Head', head_finding)

    # cv2.imshow('Tail', tail_finding)

    

    # #cv2.imshow('Original', img)

    # #cv2.imshow('Green Finding', area_finding)

    # cv2.imshow('Crop', crop_image)

    

    # while True:

        # if cv2.waitKey(1) & 0xFF == ord('q'):

            # break

i = 1

head_x,head_y,head_w,head_h = 0,0,0,0

tail_x,tail_y,tail_w,tail_h = 0,0,0,0

crop_area_min_y = 97

crop_area_max_y = 784

# 687

crop_area_min_x = 554

crop_area_max_x = 1313

# 759

crop_for_head = 30

car_position_x_centimeter_old = 0

car_position_y_centimeter_old = 0

time_stamp_old = datetime.datetime.now()

newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))

M = cv2.getPerspectiveTransform(pts1, pts2)

mapx,mapy = cv2.initUndistortRectifyMap(mtx,dist,None,newcameramtx,(w,h),5)

while True:

    ret, img = cap.read()

    time_stamp_now = datetime.datetime.now()

    #h,  w = img.shape[:2]

    #newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))

    

    #print(newcameramtx)

    

    # undistort radial distorted

    #dst = cv2.undistort(img, mtx, dist, None, newcameramtx)

    

    

    dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR)

    # crop undistorted the image

    x,y,w,h = roi

    img = dst[y:y+h, x:x+w]

    

    # undistort perspective distorted and crop aoi

    #img = dst.copy()

    [depth, width, height] = img.shape[::-1]

    

    #cv2.imshow('Before Perspective', img)

    

    

    img = cv2.warpPerspective(img,M,(width,height)) 

    #cv2.imshow('After Perspective', img)

    

    #img_res = img.copy()

    #area_finding = covEX.convertGreenPlant(img)

   

    #im2,contours,hierarchy = cv2.findContours(area_finding,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

    

    #max_x = 0

    #max_y = 0

    #min_x = 999999

    #min_y = 999999

    

    #for cnt in contours:

    #    if cv2.contourArea(cnt)>50:

    #        [x,y,w,h] = cv2.boundingRect(cnt)

            #print([x,y,w,h])

    #        if max_x < x:

    #            max_x = x + w

    #        elif min_x > x:

    #            min_x = x

    #        if max_y < y:

    #            max_y = y + h

    #        elif min_y > y:

    #            min_y = y

            #cv2.rectangle(img,(x,y),(x+w,y+h),color[i],2)

            #i += 1

    #        cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),2)

            

    #print("-------------")

    #cv2.rectangle(img,(min_x,min_y),(max_x,max_y),(255,0,0),2)

    #crop_image = img_res[min_y:max_y, min_x:max_x, :]

    #print(min_y)

    #print(max_y)

    #print(min_x)

    #print(max_x)

    #cv2.imshow('Original', img)

    

    #crop_image = img_res[crop_area_min_y:crop_area_max_y, crop_area_min_x:crop_area_max_x, :]

    crop_image = img[crop_area_min_y:crop_area_max_y, crop_area_min_x:crop_area_max_x, :]

    #crop_image_res = crop_image.copy()

    

    #tail_finding = covEX.convertGreenPlant(crop_image)

    

    tail_finding = 2.10*crop_image[:,:,1]-crop_image[:,:,2]-crop_image[:,:,0]

    tail_finding = np.where(tail_finding > 70, tail_finding, 0)

    tail_finding = np.uint8(np.abs(tail_finding))

    # tail_finding = cv2.medianBlur(tail_finding,3)

    

    tail_t,tail_contours,tail_hierarchy = cv2.findContours(tail_finding,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

            

    for cnt in tail_contours:

        if cv2.contourArea(cnt)>50:

            [tail_x,tail_y,tail_w,tail_h] = cv2.boundingRect(cnt)

            #print([tail_x,tail_y,tail_w,tail_h])

            cv2.rectangle(crop_image,(tail_x,tail_y),(tail_x+tail_w,tail_y+tail_h),(0,255,0),2)

            

    tail_position_x = tail_x + (tail_w/2)

    tail_position_y = tail_y + (tail_h/2)

    #print(tail_position_x)

    #print(tail_position_y)

    

    #head_finding = covEX.convertRed(crop_image)

    #print(tail_position_y-crop_for_head)

    #print(tail_position_y+crop_for_head)

    #print(tail_position_x-crop_for_head)

    #print(tail_position_x+crop_for_head)

    crop_head = crop_image[int(tail_position_y-crop_for_head):int(tail_position_y+crop_for_head), int(tail_position_x-crop_for_head):int(tail_position_x+crop_for_head), :]

    head_finding = 1.5*crop_head[:,:,2] - crop_head[:,:,1] - crop_head[:,:,0]

    head_finding = np.where(head_finding > 60, head_finding, 0)

    head_finding = np.uint8(np.abs(head_finding))

    #head_finding = cv2.GaussianBlur(head_finding,(9,9),0)

    #head_finding = cv2.medianBlur(head_finding,9)

    

    head_t,head_contours,head_hierarchy = cv2.findContours(head_finding,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)

    

    for cnt in head_contours:

        if cv2.contourArea(cnt)>50:

            [head_x,head_y,head_w,head_h] = cv2.boundingRect(cnt)

            #print([head_x,head_y,head_w,head_h])

            cv2.rectangle(crop_image,(int(tail_position_x) + head_x - crop_for_head, int(tail_position_y) + head_y - crop_for_head),(int(tail_position_x) + head_x+head_w - crop_for_head,int(tail_position_y) + head_y+head_h - crop_for_head),(0,255,0),2)

    

    head_position_x = int(tail_position_x) + head_x - crop_for_head + (head_w/2)

    head_position_y = int(tail_position_y) + head_y - crop_for_head + (head_h/2)

    #print(head_position_x)

    #print(head_position_y)

    

    car_position_x = (head_position_x + tail_position_x)/2

    car_position_y = (head_position_y + tail_position_y)/2

    #print(car_position_x)

    #print(car_position_y)

    

    car_orientation = math.degrees(math.atan2(head_position_y-tail_position_y, head_position_x-tail_position_x))

    if car_orientation < 0:

        car_orientation = math.fabs(car_orientation)

    else:

        car_orientation = 360 - car_orientation

    #print(car_orientation)

    

    # Convert car position to centimeter

    car_position_x_centimeter = car_position_x * 0.5151

    car_position_y_centimeter = car_position_y * 0.5247

    tail_position_x_centimeter = tail_position_y * 0.5151

    tail_position_y_centimeter = tail_position_y * 0.5247

    

    # Calculate speed of car

    # delta_s = math.sqrt(math.pow((car_position_x_centimeter_old - car_position_x_centimeter),2) + math.pow((car_position_y_centimeter_old - car_position_y_centimeter),2))

    delta_s = math.sqrt(math.pow((car_position_x_centimeter_old - tail_position_x_centimeter),2) + math.pow((car_position_y_centimeter_old - tail_position_y_centimeter),2))

    delta_t = (time_stamp_now - time_stamp_old).total_seconds()

    #print(delta_t)

    speed_of_car = delta_s / delta_t

    FPS = 1/delta_t

    

    time_stamp_old = time_stamp_now

    # car_position_x_centimeter_old = car_position_x_centimeter

    # car_position_y_centimeter_old = car_position_y_centimeter

    

    car_position_x_centimeter_old = tail_position_x_centimeter

    car_position_y_centimeter_old = tail_position_y_centimeter

    

    # Generate Text out

    car_orientation_text = "Orientation: %.2f Degrees" % car_orientation

    car_position_text = "Car position x: %.2f cm, Car position y: %.2f cm" % (car_position_x_centimeter, car_position_y_centimeter)

    car_speed_text = "Car speed: %.2f cm/s" % speed_of_car

    fps_text = "FPS: %.2f" % FPS

    

    if car_position_x_centimeter > 125-15 and car_position_x_centimeter < 125+15 and car_position_y_centimeter > 300-60 and car_position_y_centimeter < 300+60:

        print(car_speed_text)

    

    #print(car_orientation_text)

    #print(car_position_text)

    #print(car_speed_text)

    #print(fps_text)

    

    textHeight = cv2.getTextSize(car_position_text, font_face, font_scale_val, thickness_val)[0][1]

    cv2.putText(crop_image, car_position_text, (0, textHeight + offset), font_face, font_scale_val, text_color, thickness=thickness_val)

    cv2.putText(crop_image, car_orientation_text, (0, (textHeight + offset) * 2), font_face, font_scale_val, text_color, thickness=thickness_val)

    cv2.putText(crop_image, car_speed_text, (0, (textHeight + offset) * 3), font_face, font_scale_val, text_color, thickness=thickness_val)

    cv2.putText(crop_image, fps_text, (0, (textHeight + offset) * 4), font_face, font_scale_val, text_color, thickness=thickness_val)

    

    #cv2.imshow('Head', head_finding)

    #cv2.imshow('Tail', tail_finding)

    

    #cv2.imshow('Original', img)

    #cv2.imshow('Green Finding', area_finding)

    cv2.imshow('Crop', crop_image)

    

    #print(roi)

    #cv2.imshow('Original', img)

    #cv2.imshow('Calibrate', dst)

    key = cv2.waitKey(1)

    if key & 0xFF == ord('q'):

        break

        

    if key & 0xFF == ord('c'):

        filename = "Sample_img_%d.jpg" % i

        cv2.imwrite(filename, crop_image_res)

        i += 1

cap.release()

cv2.destroyAllWindows()