├── .DS_Store ├── __pycache__ ├── detector.cpython-35.pyc ├── detector_new.cpython-35.pyc ├── helpers.cpython-35.pyc └── tracker.cpython-35.pyc ├── capture_frame.py ├── detector.py ├── example_imgs ├── KF_eq_predict.gif ├── KF_predict.gif ├── KF_update.gif ├── detection_track_match.png ├── frame_01_det_track.png ├── frame_02_det_track.png ├── high_meas_noise.png ├── low_meas_noise.png ├── pred_notations.gif ├── update_notations.gif └── vehcle_detection_tracking.png ├── helpers.py ├── main.py ├── project_video.mp4 ├── readme.md ├── ssd_mobilenet_v1_coco_11_06_2017 ├── .DS_Store └── frozen_inference_graph.pb ├── test_images ├── frame001.jpg ├── frame002.jpg ├── frame003.jpg ├── frame004.jpg ├── frame005.jpg ├── frame006.jpg ├── frame007.jpg ├── frame008.jpg ├── frame009.jpg ├── frame010.jpg ├── frame011.jpg ├── frame012.jpg ├── frame013.jpg ├── frame014.jpg ├── frame015.jpg ├── frame016.jpg ├── frame017.jpg ├── frame018.jpg ├── frame019.jpg ├── frame020.jpg ├── frame021.jpg ├── frame022.jpg └── frame023.jpg ├── test_v7.mp4 └── tracker.py /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/.DS_Store -------------------------------------------------------------------------------- /__pycache__/detector.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/__pycache__/detector.cpython-35.pyc -------------------------------------------------------------------------------- /__pycache__/detector_new.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/__pycache__/detector_new.cpython-35.pyc -------------------------------------------------------------------------------- /__pycache__/helpers.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/__pycache__/helpers.cpython-35.pyc -------------------------------------------------------------------------------- /__pycache__/tracker.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/__pycache__/tracker.cpython-35.pyc -------------------------------------------------------------------------------- /capture_frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Tue Nov 22 15:07:09 2016 5 | 6 | @author: kyleguan 7 | A simple script to capture frames from a video clip 8 | 9 | """ 10 | 11 | import cv2 12 | import os 13 | 14 | video_name='project_video.mp4' 15 | start_time=12000 16 | end_time=16000 17 | step=200 18 | 19 | dir_name ='problem/' 20 | if not os.path.exists(dir_name): 21 | os.makedirs(dir_name) 22 | 23 | vidcap=cv2.VideoCapture(video_name) 24 | for i, time in enumerate(range(start_time, end_time, step)): 25 | vidcap.set(cv2.CAP_PROP_POS_MSEC, time) 26 | success, image=vidcap.read() 27 | if success: 28 | # Need to create the directory ( 'highway') first 29 | file_name='problem/frame{:03d}.jpg'.format(i+1) 30 | cv2.imwrite(file_name,image) 31 | -------------------------------------------------------------------------------- /detector.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Implement and test car detection (localization) 3 | ''' 4 | 5 | import numpy as np 6 | import tensorflow as tf 7 | from PIL import Image 8 | import os 9 | from matplotlib import pyplot as plt 10 | import time 11 | from glob import glob 12 | cwd = os.path.dirname(os.path.realpath(__file__)) 13 | 14 | # Uncomment the following two lines if need to use the Tensorflow visualization_unitls 15 | #os.chdir(cwd+'/models') 16 | #from object_detection.utils import visualization_utils as vis_util 17 | 18 | class CarDetector(object): 19 | def __init__(self): 20 | 21 | self.car_boxes = [] 22 | 23 | os.chdir(cwd) 24 | 25 | #Tensorflow localization/detection model 26 | # Single-shot-dectection with mobile net architecture trained on COCO dataset 27 | 28 | detect_model_name = 'ssd_mobilenet_v1_coco_11_06_2017' 29 | 30 | PATH_TO_CKPT = detect_model_name + '/frozen_inference_graph.pb' 31 | 32 | # setup tensorflow graph 33 | self.detection_graph = tf.Graph() 34 | 35 | # configuration for possible GPU use 36 | config = tf.ConfigProto() 37 | config.gpu_options.allow_growth = True 38 | # load frozen tensorflow detection model and initialize 39 | # the tensorflow graph 40 | with self.detection_graph.as_default(): 41 | od_graph_def = tf.GraphDef() 42 | with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid: 43 | serialized_graph = fid.read() 44 | od_graph_def.ParseFromString(serialized_graph) 45 | tf.import_graph_def(od_graph_def, name='') 46 | 47 | self.sess = tf.Session(graph=self.detection_graph, config=config) 48 | self.image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0') 49 | # Each box represents a part of the image where a particular object was detected. 50 | self.boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0') 51 | # Each score represent how level of confidence for each of the objects. 52 | # Score is shown on the result image, together with the class label. 53 | self.scores =self.detection_graph.get_tensor_by_name('detection_scores:0') 54 | self.classes = self.detection_graph.get_tensor_by_name('detection_classes:0') 55 | self.num_detections =self.detection_graph.get_tensor_by_name('num_detections:0') 56 | 57 | # Helper function to convert image into numpy array 58 | def load_image_into_numpy_array(self, image): 59 | (im_width, im_height) = image.size 60 | return np.array(image.getdata()).reshape( 61 | (im_height, im_width, 3)).astype(np.uint8) 62 | # Helper function to convert normalized box coordinates to pixels 63 | def box_normal_to_pixel(self, box, dim): 64 | 65 | height, width = dim[0], dim[1] 66 | box_pixel = [int(box[0]*height), int(box[1]*width), int(box[2]*height), int(box[3]*width)] 67 | return np.array(box_pixel) 68 | 69 | def get_localization(self, image, visual=False): 70 | 71 | """Determines the locations of the cars in the image 72 | 73 | Args: 74 | image: camera image 75 | 76 | Returns: 77 | list of bounding boxes: coordinates [y_up, x_left, y_down, x_right] 78 | 79 | """ 80 | category_index={1: {'id': 1, 'name': u'person'}, 81 | 2: {'id': 2, 'name': u'bicycle'}, 82 | 3: {'id': 3, 'name': u'car'}, 83 | 4: {'id': 4, 'name': u'motorcycle'}, 84 | 5: {'id': 5, 'name': u'airplane'}, 85 | 6: {'id': 6, 'name': u'bus'}, 86 | 7: {'id': 7, 'name': u'train'}, 87 | 8: {'id': 8, 'name': u'truck'}, 88 | 9: {'id': 9, 'name': u'boat'}, 89 | 10: {'id': 10, 'name': u'traffic light'}, 90 | 11: {'id': 11, 'name': u'fire hydrant'}, 91 | 13: {'id': 13, 'name': u'stop sign'}, 92 | 14: {'id': 14, 'name': u'parking meter'}} 93 | 94 | with self.detection_graph.as_default(): 95 | image_expanded = np.expand_dims(image, axis=0) 96 | (boxes, scores, classes, num_detections) = self.sess.run( 97 | [self.boxes, self.scores, self.classes, self.num_detections], 98 | feed_dict={self.image_tensor: image_expanded}) 99 | 100 | if visual == True: 101 | vis_util.visualize_boxes_and_labels_on_image_array( 102 | image, 103 | np.squeeze(boxes), 104 | np.squeeze(classes).astype(np.int32), 105 | np.squeeze(scores), 106 | category_index, 107 | use_normalized_coordinates=True,min_score_thresh=.4, 108 | line_thickness=3) 109 | 110 | plt.figure(figsize=(9,6)) 111 | plt.imshow(image) 112 | plt.show() 113 | 114 | boxes=np.squeeze(boxes) 115 | classes =np.squeeze(classes) 116 | scores = np.squeeze(scores) 117 | 118 | cls = classes.tolist() 119 | 120 | # The ID for car in COCO data set is 3 121 | idx_vec = [i for i, v in enumerate(cls) if ((v==3) and (scores[i]>0.3))] 122 | 123 | if len(idx_vec) ==0: 124 | print('no detection!') 125 | self.car_boxes = [] 126 | else: 127 | tmp_car_boxes=[] 128 | for idx in idx_vec: 129 | dim = image.shape[0:2] 130 | box = self.box_normal_to_pixel(boxes[idx], dim) 131 | box_h = box[2] - box[0] 132 | box_w = box[3] - box[1] 133 | ratio = box_h/(box_w + 0.01) 134 | 135 | if ((ratio < 0.8) and (box_h>20) and (box_w>20)): 136 | tmp_car_boxes.append(box) 137 | print(box, ', confidence: ', scores[idx], 'ratio:', ratio) 138 | 139 | else: 140 | print('wrong ratio or wrong size, ', box, ', confidence: ', scores[idx], 'ratio:', ratio) 141 | 142 | 143 | 144 | self.car_boxes = tmp_car_boxes 145 | 146 | return self.car_boxes 147 | 148 | if __name__ == '__main__': 149 | # Test the performance of the detector 150 | det =CarDetector() 151 | os.chdir(cwd) 152 | TEST_IMAGE_PATHS= glob(os.path.join('test_images/', '*.jpg')) 153 | 154 | for i, image_path in enumerate(TEST_IMAGE_PATHS[0:2]): 155 | print('') 156 | print('*************************************************') 157 | 158 | img_full = Image.open(image_path) 159 | img_full_np = det.load_image_into_numpy_array(img_full) 160 | img_full_np_copy = np.copy(img_full_np) 161 | start = time.time() 162 | b = det.get_localization(img_full_np, visual=False) 163 | end = time.time() 164 | print('Localization time: ', end-start) 165 | # 166 | 167 | -------------------------------------------------------------------------------- /example_imgs/KF_eq_predict.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/KF_eq_predict.gif -------------------------------------------------------------------------------- /example_imgs/KF_predict.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/KF_predict.gif -------------------------------------------------------------------------------- /example_imgs/KF_update.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/KF_update.gif -------------------------------------------------------------------------------- /example_imgs/detection_track_match.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/detection_track_match.png -------------------------------------------------------------------------------- /example_imgs/frame_01_det_track.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/frame_01_det_track.png -------------------------------------------------------------------------------- /example_imgs/frame_02_det_track.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/frame_02_det_track.png -------------------------------------------------------------------------------- /example_imgs/high_meas_noise.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/high_meas_noise.png -------------------------------------------------------------------------------- /example_imgs/low_meas_noise.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/low_meas_noise.png -------------------------------------------------------------------------------- /example_imgs/pred_notations.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/pred_notations.gif -------------------------------------------------------------------------------- /example_imgs/update_notations.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/update_notations.gif -------------------------------------------------------------------------------- /example_imgs/vehcle_detection_tracking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/example_imgs/vehcle_detection_tracking.png -------------------------------------------------------------------------------- /helpers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Helper classes and functions for detection and tracking 5 | """ 6 | 7 | import numpy as np 8 | import cv2 9 | 10 | class Box: 11 | def __init__(self): 12 | self.x, self.y = float(), float() 13 | self.w, self.h = float(), float() 14 | self.c = float() 15 | self.prob = float() 16 | 17 | def overlap(x1,w1,x2,w2): 18 | l1 = x1 - w1 / 2.; 19 | l2 = x2 - w2 / 2.; 20 | left = max(l1, l2) 21 | r1 = x1 + w1 / 2.; 22 | r2 = x2 + w2 / 2.; 23 | right = min(r1, r2) 24 | return right - left; 25 | 26 | def box_intersection(a, b): 27 | w = overlap(a.x, a.w, b.x, b.w); 28 | h = overlap(a.y, a.h, b.y, b.h); 29 | if w < 0 or h < 0: return 0; 30 | area = w * h; 31 | return area; 32 | 33 | def box_union(a, b): 34 | i = box_intersection(a, b); 35 | u = a.w * a.h + b.w * b.h - i; 36 | return u; 37 | 38 | def box_iou(a, b): 39 | return box_intersection(a, b) / box_union(a, b); 40 | 41 | def box_iou2(a, b): 42 | ''' 43 | Helper funciton to calculate the ratio between intersection and the union of 44 | two boxes a and b 45 | a[0], a[1], a[2], a[3] <-> left, up, right, bottom 46 | ''' 47 | 48 | w_intsec = np.maximum (0, (np.minimum(a[2], b[2]) - np.maximum(a[0], b[0]))) 49 | h_intsec = np.maximum (0, (np.minimum(a[3], b[3]) - np.maximum(a[1], b[1]))) 50 | s_intsec = w_intsec * h_intsec 51 | s_a = (a[2] - a[0])*(a[3] - a[1]) 52 | s_b = (b[2] - b[0])*(b[3] - b[1]) 53 | 54 | return float(s_intsec)/(s_a + s_b -s_intsec) 55 | 56 | def convert_to_pixel(box_yolo, img, crop_range): 57 | ''' 58 | Helper function to convert (scaled) coordinates of a bounding box 59 | to pixel coordinates. 60 | 61 | Example (0.89361443264143803, 0.4880486045564924, 0.23544462956491041, 62 | 0.36866588651069609) 63 | 64 | crop_range: specifies the part of image to be cropped 65 | ''' 66 | 67 | box = box_yolo 68 | imgcv = img 69 | [xmin, xmax] = crop_range[0] 70 | [ymin, ymax] = crop_range[1] 71 | h, w, _ = imgcv.shape 72 | 73 | # Calculate left, top, width, and height of the bounding box 74 | left = int((box.x - box.w/2.)*(xmax - xmin) + xmin) 75 | top = int((box.y - box.h/2.)*(ymax - ymin) + ymin) 76 | 77 | width = int(box.w*(xmax - xmin)) 78 | height = int(box.h*(ymax - ymin)) 79 | 80 | # Deal with corner cases 81 | if left < 0 : left = 0 82 | if top < 0 : top = 0 83 | 84 | # Return the coordinates (in the unit of the pixels) 85 | 86 | box_pixel = np.array([left, top, width, height]) 87 | return box_pixel 88 | 89 | 90 | 91 | def convert_to_cv2bbox(bbox, img_dim = (1280, 720)): 92 | ''' 93 | Helper fucntion for converting bbox to bbox_cv2 94 | bbox = [left, top, width, height] 95 | bbox_cv2 = [left, top, right, bottom] 96 | img_dim: dimension of the image, img_dim[0]<-> x 97 | img_dim[1]<-> y 98 | ''' 99 | left = np.maximum(0, bbox[0]) 100 | top = np.maximum(0, bbox[1]) 101 | right = np.minimum(img_dim[0], bbox[0] + bbox[2]) 102 | bottom = np.minimum(img_dim[1], bbox[1] + bbox[3]) 103 | 104 | return (left, top, right, bottom) 105 | 106 | 107 | def draw_box_label(img, bbox_cv2, box_color=(0, 255, 255), show_label=True): 108 | ''' 109 | Helper funciton for drawing the bounding boxes and the labels 110 | bbox_cv2 = [left, top, right, bottom] 111 | ''' 112 | #box_color= (0, 255, 255) 113 | font = cv2.FONT_HERSHEY_SIMPLEX 114 | font_size = 0.7 115 | font_color = (0, 0, 0) 116 | left, top, right, bottom = bbox_cv2[1], bbox_cv2[0], bbox_cv2[3], bbox_cv2[2] 117 | 118 | # Draw the bounding box 119 | cv2.rectangle(img, (left, top), (right, bottom), box_color, 4) 120 | 121 | if show_label: 122 | # Draw a filled box on top of the bounding box (as the background for the labels) 123 | cv2.rectangle(img, (left-2, top-45), (right+2, top), box_color, -1, 1) 124 | 125 | # Output the labels that show the x and y coordinates of the bounding box center. 126 | text_x= 'x='+str((left+right)/2) 127 | cv2.putText(img,text_x,(left,top-25), font, font_size, font_color, 1, cv2.LINE_AA) 128 | text_y= 'y='+str((top+bottom)/2) 129 | cv2.putText(img,text_y,(left,top-5), font, font_size, font_color, 1, cv2.LINE_AA) 130 | 131 | return img 132 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """@author: kyleguan 4 | """ 5 | 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | import glob 9 | from moviepy.editor import VideoFileClip 10 | from collections import deque 11 | from sklearn.utils.linear_assignment_ import linear_assignment 12 | 13 | import helpers 14 | import detector 15 | import tracker 16 | 17 | # Global variables to be used by funcitons of VideoFileClop 18 | frame_count = 0 # frame counter 19 | 20 | max_age = 4 # no.of consecutive unmatched detection before 21 | # a track is deleted 22 | 23 | min_hits =1 # no. of consecutive matches needed to establish a track 24 | 25 | tracker_list =[] # list for trackers 26 | # list for track ID 27 | track_id_list= deque(['A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K']) 28 | 29 | debug = True 30 | 31 | def assign_detections_to_trackers(trackers, detections, iou_thrd = 0.3): 32 | ''' 33 | From current list of trackers and new detections, output matched detections, 34 | unmatchted trackers, unmatched detections. 35 | ''' 36 | 37 | IOU_mat= np.zeros((len(trackers),len(detections)),dtype=np.float32) 38 | for t,trk in enumerate(trackers): 39 | #trk = convert_to_cv2bbox(trk) 40 | for d,det in enumerate(detections): 41 | # det = convert_to_cv2bbox(det) 42 | IOU_mat[t,d] = box_iou2(trk,det) 43 | 44 | # Produces matches 45 | # Solve the maximizing the sum of IOU assignment problem using the 46 | # Hungarian algorithm (also known as Munkres algorithm) 47 | 48 | matched_idx = linear_assignment(-IOU_mat) 49 | 50 | unmatched_trackers, unmatched_detections = [], [] 51 | for t,trk in enumerate(trackers): 52 | if(t not in matched_idx[:,0]): 53 | unmatched_trackers.append(t) 54 | 55 | for d, det in enumerate(detections): 56 | if(d not in matched_idx[:,1]): 57 | unmatched_detections.append(d) 58 | 59 | matches = [] 60 | 61 | # For creating trackers we consider any detection with an 62 | # overlap less than iou_thrd to signifiy the existence of 63 | # an untracked object 64 | 65 | for m in matched_idx: 66 | if(IOU_mat[m[0],m[1]] 0: 107 | for trk in tracker_list: 108 | x_box.append(trk.box) 109 | 110 | 111 | matched, unmatched_dets, unmatched_trks \ 112 | = assign_detections_to_trackers(x_box, z_box, iou_thrd = 0.3) 113 | if debug: 114 | print('Detection: ', z_box) 115 | print('x_box: ', x_box) 116 | print('matched:', matched) 117 | print('unmatched_det:', unmatched_dets) 118 | print('unmatched_trks:', unmatched_trks) 119 | 120 | 121 | # Deal with matched detections 122 | if matched.size >0: 123 | for trk_idx, det_idx in matched: 124 | z = z_box[det_idx] 125 | z = np.expand_dims(z, axis=0).T 126 | tmp_trk= tracker_list[trk_idx] 127 | tmp_trk.kalman_filter(z) 128 | xx = tmp_trk.x_state.T[0].tolist() 129 | xx =[xx[0], xx[2], xx[4], xx[6]] 130 | x_box[trk_idx] = xx 131 | tmp_trk.box =xx 132 | tmp_trk.hits += 1 133 | tmp_trk.no_losses = 0 134 | 135 | # Deal with unmatched detections 136 | if len(unmatched_dets)>0: 137 | for idx in unmatched_dets: 138 | z = z_box[idx] 139 | z = np.expand_dims(z, axis=0).T 140 | tmp_trk = Tracker() # Create a new tracker 141 | x = np.array([[z[0], 0, z[1], 0, z[2], 0, z[3], 0]]).T 142 | tmp_trk.x_state = x 143 | tmp_trk.predict_only() 144 | xx = tmp_trk.x_state 145 | xx = xx.T[0].tolist() 146 | xx =[xx[0], xx[2], xx[4], xx[6]] 147 | tmp_trk.box = xx 148 | tmp_trk.id = track_id_list.popleft() # assign an ID for the tracker 149 | tracker_list.append(tmp_trk) 150 | x_box.append(xx) 151 | 152 | # Deal with unmatched tracks 153 | if len(unmatched_trks)>0: 154 | for trk_idx in unmatched_trks: 155 | tmp_trk = tracker_list[trk_idx] 156 | tmp_trk.no_losses += 1 157 | tmp_trk.predict_only() 158 | xx = tmp_trk.x_state 159 | xx = xx.T[0].tolist() 160 | xx =[xx[0], xx[2], xx[4], xx[6]] 161 | tmp_trk.box =xx 162 | x_box[trk_idx] = xx 163 | 164 | 165 | # The list of tracks to be annotated 166 | good_tracker_list =[] 167 | for trk in tracker_list: 168 | if ((trk.hits >= min_hits) and (trk.no_losses <=max_age)): 169 | good_tracker_list.append(trk) 170 | x_cv2 = trk.box 171 | if debug: 172 | print('updated box: ', x_cv2) 173 | print() 174 | img= helpers.draw_box_label(img, x_cv2) # Draw the bounding boxes on the 175 | # images 176 | # Book keeping 177 | deleted_tracks = filter(lambda x: x.no_losses >max_age, tracker_list) 178 | 179 | for trk in deleted_tracks: 180 | track_id_list.append(trk.id) 181 | 182 | tracker_list = [x for x in tracker_list if x.no_losses<=max_age] 183 | 184 | if debug: 185 | print('Ending tracker_list: ',len(tracker_list)) 186 | print('Ending good tracker_list: ',len(good_tracker_list)) 187 | 188 | 189 | return img 190 | 191 | if __name__ == "__main__": 192 | 193 | det = detector.CarDetector() 194 | 195 | if debug: # test on a sequence of images 196 | images = [plt.imread(file) for file in glob.glob('./test_images/*.jpg')] 197 | 198 | for i in range(len(images))[0:7]: 199 | image = images[i] 200 | image_box = pipeline(image) 201 | plt.imshow(image_box) 202 | plt.show() 203 | 204 | else: # test on a video file. 205 | 206 | start=time.time() 207 | output = 'test_v7.mp4' 208 | clip1 = VideoFileClip("project_video.mp4")#.subclip(4,49) # The first 8 seconds doesn't have any cars... 209 | clip = clip1.fl_image(pipeline) 210 | clip.write_videofile(output, audio=False) 211 | end = time.time() 212 | 213 | print(round(end-start, 2), 'Seconds to finish') 214 | -------------------------------------------------------------------------------- /project_video.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/project_video.mp4 -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Vehicle Detection and Tracking 2 | 3 | 4 | ## Overview 5 | This repo illustrates the detection and tracking of multiple vehicles using a camera mounted inside a self-driving car. The aim here is to provide developers, researchers, and engineers a simple framework to quickly iterate different detectors and tracking algorithms. In the process, I focus on simplicity and readability of the code. The detection and tracking pipeline is relatively staight forward. It first initializes a detector and a tracker. Next, detector localizes the vehicles in each video frame. The tracker is then updated with the detection results. Finally the tracking results are annotated and displayed in a video frame. 6 | 7 | ## Key files in this repo 8 | 9 | 10 | * detector.py -- implements ```CarDetector``` class to output car detection results 11 | * tracker.py -- implements Kalman Filter-based prediction and update for tracking 12 | * main.py -- implements the detection and tracking pipeline, including detection-track assignment and track management 13 | * helpers.py -- helper functions 14 | * ssd_mobilenet_v1_coco_11_06_2017/frozen_inference_graph.pb -- pre-trained mobilenet-coco model 15 | 16 | ## Detection 17 | In the pipeline, vehicle (car) detection takes a captured image as input and produces the bounding boxes as the output. We use TensorFlow Object Detection API, which is an open source framework built on top of TensorFlow to construct, train and deploy object detection models. The Object Detection API also comes with a collection of detection models pre-trained on the COCO dataset that are well suited for fast prototyping. Specifically, we use a lightweight model: ssd\_mobilenet\_v1\_coco that is based on Single Shot Multibox Detection (SSD) framework with minimal modification. Though this is a general-purpose detection model (not specifically optimized for vehicle detection), we find this model achieves the balance between bounding box accuracy and inference time. 18 | 19 | The detector is implemented in ```CarDetector``` class in detector.py. The output are the coordinates of the bounding boxes (in the format of [y\_up, x\_left, y\_down, x\_right] ) of all the detected vehicles. 20 | 21 | The COCO dataset contains images of 90 classes, with the first 14 classes all related to transportation, including bicycle, car, and bus, etc. The ID for car is 3. 22 | 23 | ``` 24 | category_index={1: {'id': 1, 'name': u'person'}, 25 | 2: {'id': 2, 'name': u'bicycle'}, 26 | 3: {'id': 3, 'name': u'car'}, 27 | 4: {'id': 4, 'name': u'motorcycle'}, 28 | 5: {'id': 5, 'name': u'airplane'}, 29 | 6: {'id': 6, 'name': u'bus'}, 30 | 7: {'id': 7, 'name': u'train'}, 31 | 8: {'id': 8, 'name': u'truck'}, 32 | 9: {'id': 9, 'name': u'boat'}, 33 | 10: {'id': 10, 'name': u'traffic light'}, 34 | 11: {'id': 11, 'name': u'fire hydrant'}, 35 | 13: {'id': 13, 'name': u'stop sign'}, 36 | 14: {'id': 14, 'name': u'parking meter'}} 37 | ``` 38 | The following code snippet implements the actual detection using TensorFlow API. 39 | 40 | ``` 41 | (boxes, scores, classes, num_detections) = self.sess.run( 42 | [self.boxes, self.scores, self.classes, self.num_detections], 43 | feed_dict={self.image_tensor: image_expanded}) 44 | ``` 45 | Here ```boxes```, ```scores```, and ```classes``` represent the bounding box, confidence level, and class name corresponding to each of the detection, respectively. Next, we select the detections that are cars and have a confidence greater than a threshold ( e.g., 0.3 in this case). 46 | ``` 47 | idx_vec = [i for i, v in enumerate(cls) if ((v==3) and (scores[i]>0.3))] 48 | ``` 49 | To detect all kinds of vehicles, we also include the indices for bus and truck. 50 | ``` 51 | idx_vec = [i for i, v in enumerate(cls) if (((v==3) or (v==6) or (v==8)) and (scores[i]>0.3))] 52 | ``` 53 | To further reduce possible false positives, we include thresholds for bounding box width, height, and height-to-width ratio. 54 | 55 | ``` 56 | if ((ratio < 0.8) and (box_h>20) and (box_w>20)): 57 | tmp_car_boxes.append(box) 58 | print(box, ', confidence: ', scores[idx], 'ratio:', ratio) 59 | else: 60 | print('wrong ratio or wrong size, ', box, ', confidence: ', scores[idx], 'ratio:', ratio) 61 | ``` 62 | 63 | ## Kalman Filter for Bounding Box Measurement 64 | 65 | We use Kalman filter for tracking objects. Kalman filter has the following important features that tracking can benefit from: 66 | 67 | * Prediction of object's future location 68 | * Correction of the prediction based on new measurements 69 | * Reduction of noise introduced by inaccurate detections 70 | * Facilitating the process of association of multiple objects to their tracks 71 | 72 | Kalman filter consists of two steps: prediction and update. The first step uses previous states to predict the current state. The second step uses the current measurement, such as detection bounding box location , to correct the state. The formula are provided in the following: 73 | 74 | ### Kalman Filter Equations: 75 | #### Prediction phase: notations 76 | Drawing 77 | #### Prediction phase: equations 78 | Drawing 79 | #### Update phase: notations 80 | Drawing 81 | #### Update phase: equations 82 | Drawing 83 | 84 | ### Kalman Filter Implementation 85 | In this section, we describe the implementation of the Kalman filter in detail. 86 | 87 | The state vector has eight elements as follows: 88 | ``` 89 | [up, up_dot, left, left_dot, down, down_dot, right, right_dot] 90 | ``` 91 | That is, we use the coordinates and their first-order derivatives of the up left corner and lower right corner of the bounding box. 92 | 93 | The process matrix, assuming the constant velocity (thus no acceleration), is: 94 | 95 | ``` 96 | self.F = np.array([[1, self.dt, 0, 0, 0, 0, 0, 0], 97 | [0, 1, 0, 0, 0, 0, 0, 0], 98 | [0, 0, 1, self.dt, 0, 0, 0, 0], 99 | [0, 0, 0, 1, 0, 0, 0, 0], 100 | [0, 0, 0, 0, 1, self.dt, 0, 0], 101 | [0, 0, 0, 0, 0, 1, 0, 0], 102 | [0, 0, 0, 0, 0, 0, 1, self.dt], 103 | [0, 0, 0, 0, 0, 0, 0, 1]]) 104 | ``` 105 | The measurement matrix, given that the detector only outputs the coordindate (not velocity), is: 106 | 107 | ``` 108 | self.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0], 109 | [0, 0, 1, 0, 0, 0, 0, 0], 110 | [0, 0, 0, 0, 1, 0, 0, 0], 111 | [0, 0, 0, 0, 0, 0, 1, 0]]) 112 | ``` 113 | The state, process, and measurement noises are : 114 | 115 | ``` 116 | # Initialize the state covariance 117 | self.L = 100.0 118 | self.P = np.diag(self.L*np.ones(8)) 119 | 120 | 121 | # Initialize the process covariance 122 | self.Q_comp_mat = np.array([[self.dt**4/2., self.dt**3/2.], 123 | [self.dt**3/2., self.dt**2]]) 124 | self.Q = block_diag(self.Q_comp_mat, self.Q_comp_mat, 125 | self.Q_comp_mat, self.Q_comp_mat) 126 | 127 | # Initialize the measurement covariance 128 | self.R_scaler = 1.0/16.0 129 | self.R_diag_array = self.R_ratio * np.array([self.L, self.L, self.L, self.L]) 130 | self.R = np.diag(self.R_diag_array) 131 | ``` 132 | Here ```self.R_scaler``` represents the "magnitude" of measurement noise relative to state noise. A low ```self.R_scaler``` indicates a more reliable measurement. The following figures visualize the impact of measurement noise to the Kalman filter process. The green bounding box represents the prediction (initial) state. The red bounding box represents the measurement. 133 | If measurement noise is low, the updated state (aqua colored bounding box) is very close to the measurement (aqua bounding box completely overlaps over the red bounding box). 134 | 135 | Drawing 136 | 137 | In contrast, if measurement noise is high, the updated state is very close to the initial prediction (aqua bounding box completely overlaps over the green bounding box). 138 | 139 | Drawing 140 | 141 | ## Detection-to-Tracker Assignment 142 | 143 | The module ```assign_detections_to_trackers(trackers, detections, iou_thrd = 0.3)``` takes from current list of trackers and new detections, output matched detections, unmatched trackers, unmatched detections. 144 | 145 | Drawing 146 | 147 | ### Linear Assignment and Hungarian (Munkres) algorithm 148 | 149 | If there are multiple detections, we need to match (assign) each of them to a tracker. We use intersection over union (IOU) of a tracker bounding box and detection bounding box as a metric. We solve the maximizing the sum of IOU assignment problem using the Hungarian algorithm (also known as Munkres algorithm). The machine learning package scikit-learn has a build-in utility function that implements the Hungarian algorithm. 150 | 151 | ``` 152 | matched_idx = linear_assignment(-IOU_mat) 153 | ``` 154 | Note that ```linear_assignment ``` by default minimizes an objective function. So we need to reverse the sign of ```IOU_mat``` for maximization. 155 | 156 | ### Unmatched detections and trackers 157 | 158 | Based on the linear assignment results, we keep two lists for unmatched detections and unmatched trackers, respectively. When a car enters into a frame and is first detected, it is not matched with any existing tracks, thus this particular detection is referred to as an unmatched detection, as shown in the following figure. In addition, any matching with an overlap less than ```iou_thrd``` signifies the existence of 159 | an untracked object. When a car leaves the frame, the previously established track has no more detection to associate with. In this scenario, the track is referred to as unmatched track. Thus, the tracker and the detection associated in the matching are added to the lists of unmatched trackers and unmatched detection, respectively. 160 | 161 | Drawing 162 | 163 | ## Pipeline 164 | 165 | We include two important design parameters, ```min_hits``` and ```max_age```, in the pipeline. The parameter ```min_hits``` is the number of consecutive matches needed to establish a track. The parameter ```max_age``` is number of consecutive unmatched detections before a track is deleted. Both parameters need to be tuned to improve the tracking and detection performance. 166 | 167 | The pipeline deals with matched detection, unmatched detection, and unmatched trackers sequentially. We annotate the tracks that meet the ```min_hits``` and ```max_age``` condition. Proper book keeping is also needed to delete the stale tracks. 168 | 169 | The following examples show the process of the pipeline. When the car is first detected in the first video frame, running the following line of code returns an empty list, an one-element list, and an empty list for ```matched```, ```unmatched_dets```, and ```unmatched_trks```, respectively. 170 | 171 | ``` 172 | matched, unmatched_dets, unmatched_trks \ 173 | = assign_detections_to_trackers(x_box, z_box, iou_thrd = 0.3) 174 | ``` 175 | We thus have a situation of unmatched detections. Unmatched detections are processed by the following code block: 176 | 177 | ``` 178 | if len(unmatched_dets)>0: 179 | for idx in unmatched_dets: 180 | z = z_box[idx] 181 | z = np.expand_dims(z, axis=0).T 182 | tmp_trk = Tracker() # Create a new tracker 183 | x = np.array([[z[0], 0, z[1], 0, z[2], 0, z[3], 0]]).T 184 | tmp_trk.x_state = x 185 | tmp_trk.predict_only() 186 | xx = tmp_trk.x_state 187 | xx = xx.T[0].tolist() 188 | xx =[xx[0], xx[2], xx[4], xx[6]] 189 | tmp_trk.box = xx 190 | tmp_trk.id = track_id_list.popleft() # assign an ID for the tracker 191 | tracker_list.append(tmp_trk) 192 | x_box.append(xx) 193 | ``` 194 | This code block carries out two important tasks, 1) creating a new tracker ```tmp_trk``` for the detection; 2) carrying out the Kalman filter's predict stage ```tmp_trk.predict_only()```. Note that this newly created track is still in probation period, i.e., ```trk.hits =0```, so this track is yet established at the end of pipeline. The output image is the same as the input image - the detection bounding box is not annotated. 195 | Drawing 196 | 197 | When the car is detected again in the second video frame, running the following ```assign_detections_to_trackers``` returns an one-element list , an empty list, and an empty list for matched, unmatched_dets, and unmatched_trks, respectively. As shown in the following figure, we have a matched detection, which will be processed by the following code block: 198 | 199 | ``` 200 | if matched.size >0: 201 | for trk_idx, det_idx in matched: 202 | z = z_box[det_idx] 203 | z = np.expand_dims(z, axis=0).T 204 | tmp_trk= tracker_list[trk_idx] 205 | tmp_trk.kalman_filter(z) 206 | xx = tmp_trk.x_state.T[0].tolist() 207 | xx =[xx[0], xx[2], xx[4], xx[6]] 208 | x_box[trk_idx] = xx 209 | tmp_trk.box =xx 210 | tmp_trk.hits += 1 211 | ``` 212 | This code block carries out two important tasks, 1) carrying out the Kalman filter's prediction and update stages ```tmp_trk.kalman_filter()```; 2) increasing the hits of the track by one ```tmp_trk.hits +=1```. With this update, 213 | the condition ```if ((trk.hits >= min_hits) and (trk.no_losses <=max_age)) ``` is statified, so the track is fully established. As the result, the bounding box is annotated in the output image, as shown in the figure below. 214 | Drawing 215 | ## Issues 216 | 217 | The main issue is occlusion. For example, when one car is passing another car, the two cars can be very close to each other. This can fool the detector into outputing a single (and possibly bigger bounding) box, instead of two separate bounding boxes. In addition, the tracking algorithm may treat this detection as a new detection and sets up a new track. The tracking algorithm may fail again when one of the passing car moves away from another car. 218 | 219 | 220 | -------------------------------------------------------------------------------- /ssd_mobilenet_v1_coco_11_06_2017/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/ssd_mobilenet_v1_coco_11_06_2017/.DS_Store -------------------------------------------------------------------------------- /ssd_mobilenet_v1_coco_11_06_2017/frozen_inference_graph.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/ssd_mobilenet_v1_coco_11_06_2017/frozen_inference_graph.pb -------------------------------------------------------------------------------- /test_images/frame001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame001.jpg -------------------------------------------------------------------------------- /test_images/frame002.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame002.jpg -------------------------------------------------------------------------------- /test_images/frame003.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame003.jpg -------------------------------------------------------------------------------- /test_images/frame004.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame004.jpg -------------------------------------------------------------------------------- /test_images/frame005.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame005.jpg -------------------------------------------------------------------------------- /test_images/frame006.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame006.jpg -------------------------------------------------------------------------------- /test_images/frame007.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame007.jpg -------------------------------------------------------------------------------- /test_images/frame008.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame008.jpg -------------------------------------------------------------------------------- /test_images/frame009.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame009.jpg -------------------------------------------------------------------------------- /test_images/frame010.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame010.jpg -------------------------------------------------------------------------------- /test_images/frame011.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame011.jpg -------------------------------------------------------------------------------- /test_images/frame012.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame012.jpg -------------------------------------------------------------------------------- /test_images/frame013.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame013.jpg -------------------------------------------------------------------------------- /test_images/frame014.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame014.jpg -------------------------------------------------------------------------------- /test_images/frame015.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame015.jpg -------------------------------------------------------------------------------- /test_images/frame016.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame016.jpg -------------------------------------------------------------------------------- /test_images/frame017.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame017.jpg -------------------------------------------------------------------------------- /test_images/frame018.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame018.jpg -------------------------------------------------------------------------------- /test_images/frame019.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame019.jpg -------------------------------------------------------------------------------- /test_images/frame020.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame020.jpg -------------------------------------------------------------------------------- /test_images/frame021.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame021.jpg -------------------------------------------------------------------------------- /test_images/frame022.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame022.jpg -------------------------------------------------------------------------------- /test_images/frame023.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_images/frame023.jpg -------------------------------------------------------------------------------- /test_v7.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kcg2015/Vehicle-Detection-and-Tracking/0c1c506c2ea23b3f7fe9e9c7c2c7aaa70fd5486b/test_v7.mp4 -------------------------------------------------------------------------------- /tracker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | ''' 4 | Implement and test tracker 5 | ''' 6 | import numpy as np 7 | from numpy import dot 8 | from scipy.linalg import inv, block_diag 9 | 10 | 11 | 12 | class Tracker(): # class for Kalman Filter-based tracker 13 | def __init__(self): 14 | # Initialize parametes for tracker (history) 15 | self.id = 0 # tracker's id 16 | self.box = [] # list to store the coordinates for a bounding box 17 | self.hits = 0 # number of detection matches 18 | self.no_losses = 0 # number of unmatched tracks (track loss) 19 | 20 | # Initialize parameters for Kalman Filtering 21 | # The state is the (x, y) coordinates of the detection box 22 | # state: [up, up_dot, left, left_dot, down, down_dot, right, right_dot] 23 | # or[up, up_dot, left, left_dot, height, height_dot, width, width_dot] 24 | self.x_state=[] 25 | self.dt = 1. # time interval 26 | 27 | # Process matrix, assuming constant velocity model 28 | self.F = np.array([[1, self.dt, 0, 0, 0, 0, 0, 0], 29 | [0, 1, 0, 0, 0, 0, 0, 0], 30 | [0, 0, 1, self.dt, 0, 0, 0, 0], 31 | [0, 0, 0, 1, 0, 0, 0, 0], 32 | [0, 0, 0, 0, 1, self.dt, 0, 0], 33 | [0, 0, 0, 0, 0, 1, 0, 0], 34 | [0, 0, 0, 0, 0, 0, 1, self.dt], 35 | [0, 0, 0, 0, 0, 0, 0, 1]]) 36 | 37 | # Measurement matrix, assuming we can only measure the coordinates 38 | 39 | self.H = np.array([[1, 0, 0, 0, 0, 0, 0, 0], 40 | [0, 0, 1, 0, 0, 0, 0, 0], 41 | [0, 0, 0, 0, 1, 0, 0, 0], 42 | [0, 0, 0, 0, 0, 0, 1, 0]]) 43 | 44 | 45 | # Initialize the state covariance 46 | self.L = 10.0 47 | self.P = np.diag(self.L*np.ones(8)) 48 | 49 | 50 | # Initialize the process covariance 51 | self.Q_comp_mat = np.array([[self.dt**4/4., self.dt**3/2.], 52 | [self.dt**3/2., self.dt**2]]) 53 | self.Q = block_diag(self.Q_comp_mat, self.Q_comp_mat, 54 | self.Q_comp_mat, self.Q_comp_mat) 55 | 56 | # Initialize the measurement covariance 57 | self.R_scaler = 1.0 58 | self.R_diag_array = self.R_scaler * np.array([self.L, self.L, self.L, self.L]) 59 | self.R = np.diag(self.R_diag_array) 60 | 61 | 62 | def update_R(self): 63 | R_diag_array = self.R_scaler * np.array([self.L, self.L, self.L, self.L]) 64 | self.R = np.diag(R_diag_array) 65 | 66 | 67 | 68 | 69 | def kalman_filter(self, z): 70 | ''' 71 | Implement the Kalman Filter, including the predict and the update stages, 72 | with the measurement z 73 | ''' 74 | x = self.x_state 75 | # Predict 76 | x = dot(self.F, x) 77 | self.P = dot(self.F, self.P).dot(self.F.T) + self.Q 78 | 79 | #Update 80 | S = dot(self.H, self.P).dot(self.H.T) + self.R 81 | K = dot(self.P, self.H.T).dot(inv(S)) # Kalman gain 82 | y = z - dot(self.H, x) # residual 83 | x += dot(K, y) 84 | self.P = self.P - dot(K, self.H).dot(self.P) 85 | self.x_state = x.astype(int) # convert to integer coordinates 86 | #(pixel values) 87 | 88 | def predict_only(self): 89 | ''' 90 | Implment only the predict stage. This is used for unmatched detections and 91 | unmatched tracks 92 | ''' 93 | x = self.x_state 94 | # Predict 95 | x = dot(self.F, x) 96 | self.P = dot(self.F, self.P).dot(self.F.T) + self.Q 97 | self.x_state = x.astype(int) 98 | 99 | if __name__ == "__main__": 100 | 101 | import matplotlib.pyplot as plt 102 | import glob 103 | import helpers 104 | 105 | # Creat an instance 106 | trk = Tracker() 107 | # Test R_ratio 108 | trk.R_scaler = 1.0/16 109 | # Update measurement noise covariance matrix 110 | trk.update_R() 111 | # Initial state 112 | x_init = np.array([390, 0, 1050, 0, 513, 0, 1278, 0]) 113 | x_init_box = [x_init[0], x_init[2], x_init[4], x_init[6]] 114 | # Measurement 115 | z=np.array([399, 1022, 504, 1256]) 116 | trk.x_state= x_init.T 117 | trk.kalman_filter(z.T) 118 | # Updated state 119 | x_update =trk.x_state 120 | x_updated_box = [x_update[0], x_update[2], x_update[4], x_update[6]] 121 | 122 | print('The initial state is: ', x_init) 123 | print('The measurement is: ', z) 124 | print('The update state is: ', x_update) 125 | 126 | # Visualize the Kalman filter process and the 127 | # impact of measurement nosie convariance matrix 128 | 129 | images = [plt.imread(file) for file in glob.glob('./test_images/*.jpg')] 130 | img=images[3] 131 | 132 | plt.figure(figsize=(10, 14)) 133 | helpers.draw_box_label(img, x_init_box, box_color=(0, 255, 0)) 134 | ax = plt.subplot(3, 1, 1) 135 | plt.imshow(img) 136 | plt.title('Initial: '+str(x_init_box)) 137 | 138 | helpers.draw_box_label(img, z, box_color=(255, 0, 0)) 139 | ax = plt.subplot(3, 1, 2) 140 | plt.imshow(img) 141 | plt.title('Measurement: '+str(z)) 142 | 143 | helpers.draw_box_label(img, x_updated_box) 144 | ax = plt.subplot(3, 1, 3) 145 | plt.imshow(img) 146 | plt.title('Updated: '+str(x_updated_box)) 147 | plt.show() 148 | --------------------------------------------------------------------------------