From eb9e73723dcdc37c82df05ac5459624bd1f0ec7b Mon Sep 17 00:00:00 2001
From: RogerQi <17267101+RogerQi@users.noreply.github.com>
Date: Thu, 27 Oct 2022 15:23:58 -0500
Subject: [PATCH 01/25] add PoC SHL_200WS camera 120fps pipeline based on
 gstreamer

---
 Camera/shl_200ws.py | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)
 create mode 100644 Camera/shl_200ws.py

diff --git a/Camera/shl_200ws.py b/Camera/shl_200ws.py
new file mode 100644
index 0000000..a1ba2df
--- /dev/null
+++ b/Camera/shl_200ws.py
@@ -0,0 +1,33 @@
+import numpy as np
+import cv2
+import time
+
+cap = cv2.VideoCapture('v4l2src device=/dev/video0 ! image/jpeg,framerate=61612/513,width=640,height=480 ! jpegdec ! videoconvert ! video/x-raw,format=BGR ! appsink max-buffers=1 drop=true', cv2.CAP_GSTREAMER)
+
+if not cap.isOpened():
+    print("Cannot open camera")
+    exit()
+
+while True:
+    start_cp = time.time()
+    # Capture frame-by-frame
+    ret, frame = cap.read()
+    # if frame is read correctly ret is True
+    if not ret:
+        print("Can't receive frame (stream end?). Exiting ...")
+        break
+    print(frame.shape)
+    # Our operations on the frame come here
+    # Display the resulting frame
+    cv2.imshow('frame', frame)
+    my_key = cv2.waitKey(1)
+    if my_key == ord('q'):
+        break
+    elif my_key == ord('s'):
+        cv2.imwrite('test.jpg', frame)
+    print("Total time elapsed: {:.4f}".format(time.time() - start_cp))
+
+# When everything done, release the capture
+
+cap.release()
+cv2.destroyAllWindows()

From a4f3ad0230deef3d1aa3b1e7c143ba71ec74808d Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 18 Dec 2022 15:39:42 -0600
Subject: [PATCH 02/25] add brightening routine in auto brightness alignment

---
 Detection/CV_mix_DL.py | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 12d7b8b..2214b0a 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -20,6 +20,13 @@ def auto_align_brightness(img, target_v=50):
         img = cv2.cvtColor(final_hsv, cv2.COLOR_HSV2BGR)
         return img
     else:
+        # brighten
+        value = -v_diff
+        # needs lower brightness
+        v[v > (255 - value)] = 255
+        v[v <= (255 - value)] += value
+        final_hsv = cv2.merge((h, s, v))
+        img = cv2.cvtColor(final_hsv, cv2.COLOR_HSV2BGR)
         return img
 
 class cv_mix_dl_detector:

From f96233879bcda277266990a35c8205c08d13e1f3 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 18 Dec 2022 15:42:16 -0600
Subject: [PATCH 03/25] add read_from_file fake camera for easy dev

---
 Camera/read_from_file.py | 40 ++++++++++++++++++++++++++++++++++++++++
 config.py                |  7 ++++---
 2 files changed, 44 insertions(+), 3 deletions(-)
 create mode 100644 Camera/read_from_file.py

diff --git a/Camera/read_from_file.py b/Camera/read_from_file.py
new file mode 100644
index 0000000..1743e46
--- /dev/null
+++ b/Camera/read_from_file.py
@@ -0,0 +1,40 @@
+# Read from file
+import sys
+import numpy as np
+import cv2
+import Utils
+
+# TODO: make an abstract camera base class
+class fake_camera(object):
+    # Needs to be calibrated per camera
+    YAW_FOV_HALF = Utils.deg_to_rad(42) / 2
+    PITCH_FOV_HALF = Utils.deg_to_rad(42) / 2
+
+    def __init__(self, width, height):
+        self.width = width
+        self.height = height
+
+        assert len(sys.argv) == 2 # main py file; video file
+        video_path = sys.argv[1]
+        assert video_path.endswith('.mp4')
+
+        self.cap = cv2.VideoCapture(video_path)
+        assert self.cap.isOpened()
+
+        self.alive = True # init to True
+
+    def get_frame(self):
+        if not self.cap.isOpened():
+            self.alive = False
+        
+        ret, frame = self.cap.read()
+
+        if not ret:
+            self.alive = False
+        
+        if not self.alive:
+            raise
+        
+        frame = cv2.resize(frame, (self.width, self.height))
+
+        return (frame, np.ones_like(frame))
diff --git a/config.py b/config.py
index c8bc21a..d4dbc2b 100644
--- a/config.py
+++ b/config.py
@@ -9,12 +9,13 @@
 MOVE_YOKE = b'MY'
 PACK_END = b'ED'
 
-from Camera.d455 import D455_camera
+# from Camera.d455 import D455_camera
+from Camera.read_from_file import fake_camera
 
-RGBD_CAMERA = D455_camera
+RGBD_CAMERA = fake_camera
 
 # Compute some constants and define camera to use
-IMG_HEIGHT=480
+IMG_HEIGHT=360
 IMG_WIDTH=640
 
 IMG_CENTER_X = IMG_WIDTH // 2

From ebe7bce419eaddc07de4655630c82ef858aa952f Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 18 Dec 2022 15:44:47 -0600
Subject: [PATCH 04/25] allow execution without USB serial

---
 vision.py | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/vision.py b/vision.py
index 9310fdd..3785bd8 100644
--- a/vision.py
+++ b/vision.py
@@ -8,7 +8,7 @@
 
 DEFAULT_ENEMY_TEAM = 'red'
 
-DEBUG_DISPLAY = False
+DEBUG_DISPLAY = True
 
 class serial_circular_buffer:
     def __init__(self, buffer_size=10):
@@ -54,14 +54,18 @@ def get_enemy_color(self):
     # color buffer which retrieves enemy color from STM32
     my_color_buffer = serial_circular_buffer()
 
+    if communicator is None:
+        print("SERIAL DEVICE IS NOT AVAILABLE!!!")
+
     while True:
         start = time.time()
         frame, depth = rgbd_camera.get_frame()
 
-        if (communicator.inWaiting() > 0):
-            # read the bytes and convert from binary array to ASCII
-            byte_array = communicator.read(communicator.inWaiting())
-            my_color_buffer.receive(byte_array)
+        if communicator is not None:
+            if (communicator.inWaiting() > 0):
+                # read the bytes and convert from binary array to ASCII
+                byte_array = communicator.read(communicator.inWaiting())
+                my_color_buffer.receive(byte_array)
         
         enemy_team = my_color_buffer.get_enemy_color()
         model.change_color(enemy_team)
@@ -92,7 +96,5 @@ def get_enemy_color(self):
 
         if communicator is not None:
             communicator.write(packet)
-        else:
-            print("PACKET CREATED BUT SERIAL DEVICE IS NOT AVAILABLE!!!")
 
         pkt_seq += 1

From 2e88e95c93e25d454450912a697d5efbd892ea18 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 18 Dec 2022 15:52:11 -0600
Subject: [PATCH 05/25] update README

---
 README.md | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index 5417a9f..3be3e94 100644
--- a/README.md
+++ b/README.md
@@ -1,11 +1,17 @@
 # iRM_Vision_2022
 repo for iRM vision.
 
-## build guide:
+## Data preparing
 
-!TODO
+We recorded some demo videos to demo auto-aiming capabilities of our robots,
+which is located under the folder `./large_data/`.
+However, due to the large sizes of the video, it's inappropriate to directly
+upload them to GitHub. Hence, to acquire theese sample videos, please download
+them at the [UofI box][https://uofi.box.com/s/i6zahotr9id35hurjzy2bq3dcfz0085e].
 
-I haven't try it yet. Shouldn't be a big issue though...
+## Dependencies
+
+Please follow instruction from the RMCV101 repo [here](https://github.com/illini-robomaster/RM_CV_101/blob/master/INSTALL.md).
 
 ## File Structure
 

From b237993ed8c6ecec9ba93515d5244a8b90c7c8f5 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sat, 28 Jan 2023 02:16:25 -0600
Subject: [PATCH 06/25] Implement basic tracker; move byte str decoding logic
 to vision.py

---
 Aiming/Aim.py                    | 18 +++++++------
 Aiming/Tracking/__init__.py      |  2 +-
 Aiming/Tracking/basic_tracker.py | 43 ++++++++++++++++++++++++++++++++
 vision.py                        | 13 +++++++++-
 4 files changed, 66 insertions(+), 10 deletions(-)
 create mode 100644 Aiming/Tracking/basic_tracker.py

diff --git a/Aiming/Aim.py b/Aiming/Aim.py
index 54944ff..9beba6e 100644
--- a/Aiming/Aim.py
+++ b/Aiming/Aim.py
@@ -2,12 +2,19 @@
 import config
 import Utils
 
+from .Tracking import basic_tracker
+
 class Aim:
     def __init__(self):
-        pass
+        self.tracker = basic_tracker()
 
-    def process_one(self, pred_list, enemy_team, depth_map):
+    def process_one(self, pred_list, enemy_team, rgb_img, depth_map):
         assert enemy_team in ['blue', 'red']
+
+        pred_list = self.tracker.fix_prediction(pred_list)
+
+        self.tracker.register_one(pred_list, enemy_team, rgb_img, depth_map)
+
         closet_pred, closet_dist = self.get_closet_pred(pred_list, enemy_team, depth_map)
         if closet_pred is None:
             return None
@@ -51,12 +58,7 @@ def get_closet_pred(self, pred_list, enemy_team, depth_map):
         closet_dist = None # Cloest to camera in z-axis
         obj_of_interest = [f"armor_{enemy_team}"]
         for name, conf, bbox in pred_list:
-            # name from C++ string is in bytes; decoding is needed
-            if isinstance(name, bytes):
-                name_str = name.decode('utf-8')
-            else:
-                name_str = name
-            if name_str not in obj_of_interest: continue
+            if name not in obj_of_interest: continue
             cur_dist = Utils.estimate_target_depth(bbox, depth_map)
             if closet_pred is None:
                 closet_pred = (name, conf, bbox)
diff --git a/Aiming/Tracking/__init__.py b/Aiming/Tracking/__init__.py
index fc6bb0e..2d2f5c0 100644
--- a/Aiming/Tracking/__init__.py
+++ b/Aiming/Tracking/__init__.py
@@ -1 +1 @@
-#placeholder
\ No newline at end of file
+from .basic_tracker import basic_tracker
\ No newline at end of file
diff --git a/Aiming/Tracking/basic_tracker.py b/Aiming/Tracking/basic_tracker.py
new file mode 100644
index 0000000..4097a43
--- /dev/null
+++ b/Aiming/Tracking/basic_tracker.py
@@ -0,0 +1,43 @@
+import os
+import numpy as np
+
+class basic_tracker(object):
+    '''
+    Basic tracker that can handle only one target.
+
+    It memorizes the state of last two predictions and do linear extrapolation
+    '''
+    def __init__(self):
+        self.last_pred = None
+        self.last_last_pred = None
+
+    def register_one(self, pred_list, enemy_team, rgb_img, depth_map):
+        # TODO: add a tracking flag in pred_list to distinguish
+        # tracked and detected objects
+        my_pred_list = [i for i in pred_list if i[0] == f"armor_{enemy_team}"]
+
+        assert len(my_pred_list) <= 1, "Basic tracker does not support multiple targets"
+
+        self.last_last_pred = self.last_pred
+        self.last_pred = my_pred_list
+
+    def fix_prediction(self, cur_pred_list):
+        if len(cur_pred_list) == 0:
+            last_name, last_conf, last_bbox = self.last_pred[0]
+            last_last_name, last_last_conf, last_last_bbox = self.last_last_pred[0]
+
+            assert last_name == last_last_name
+
+            last_center_x, last_center_y, last_width, last_height = last_bbox
+            last_last_center_x, last_last_center_y, last_last_width, last_last_height = last_last_bbox
+
+            # Linear extrapolation
+            center_x = last_center_x + (last_center_x - last_last_center_x) / 2
+            center_y = last_center_y + (last_center_y - last_last_center_y) / 2
+            width = last_width + (last_width - last_last_width) / 2
+            height = last_height + (last_height - last_last_height) / 2
+
+            return [[last_name, (last_conf + last_last_conf) / 2, [center_x, center_y, width, height]]]
+        else:
+            # If not empty, do nothing
+            return cur_pred_list
diff --git a/vision.py b/vision.py
index 3785bd8..612a4e3 100644
--- a/vision.py
+++ b/vision.py
@@ -71,9 +71,20 @@ def get_enemy_color(self):
         model.change_color(enemy_team)
 
         pred = model.detect(frame)
+
+        for i in range(len(pred)):
+            name, conf, bbox = pred[i]
+            # name from C++ string is in bytes; decoding is needed
+            if isinstance(name, bytes):
+                name_str = name.decode('utf-8')
+            else:
+                name_str = name
+            pred[i] = (name_str, conf, bbox)
+
         elapsed = time.time()-start
         
-        ret_dict = aimer.process_one(pred, enemy_team, depth)
+        # Tracking and filtering
+        ret_dict = aimer.process_one(pred, enemy_team, frame, depth)
 
         show_frame = frame.copy()
 

From b09167ee8e6e1b4499b8eb9378e05856598f01f8 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sat, 28 Jan 2023 02:28:14 -0600
Subject: [PATCH 07/25] Completely remove depth map input and deprecate D455
 reader

---
 Aiming/Aim.py                          | 21 ++++++++++++++-------
 Aiming/Tracking/basic_tracker.py       |  2 +-
 Camera/{d455.py => d455.py.deprecated} |  0
 Camera/read_from_file.py               |  4 ++--
 Utils/__init__.py                      | 24 ------------------------
 config.py                              |  2 +-
 vision.py                              |  6 +++---
 7 files changed, 21 insertions(+), 38 deletions(-)
 rename Camera/{d455.py => d455.py.deprecated} (100%)

diff --git a/Aiming/Aim.py b/Aiming/Aim.py
index 9beba6e..374354e 100644
--- a/Aiming/Aim.py
+++ b/Aiming/Aim.py
@@ -8,14 +8,14 @@ class Aim:
     def __init__(self):
         self.tracker = basic_tracker()
 
-    def process_one(self, pred_list, enemy_team, rgb_img, depth_map):
+    def process_one(self, pred_list, enemy_team, rgb_img):
         assert enemy_team in ['blue', 'red']
 
         pred_list = self.tracker.fix_prediction(pred_list)
 
-        self.tracker.register_one(pred_list, enemy_team, rgb_img, depth_map)
+        self.tracker.register_one(pred_list, enemy_team, rgb_img)
 
-        closet_pred, closet_dist = self.get_closet_pred(pred_list, enemy_team, depth_map)
+        closet_pred, closet_dist = self.get_closet_pred(pred_list, enemy_team, rgb_img)
         if closet_pred is None:
             return None
         name, confidence, bbox = closet_pred
@@ -53,13 +53,20 @@ def posterior_calibration(self, yaw_diff, pitch_diff, distance):
             # TODO: compute a range table
             return (yaw_diff, pitch_diff)
     
-    def get_closet_pred(self, pred_list, enemy_team, depth_map):
+    def get_closet_pred(self, pred_list, enemy_team, rgb_img):
+        '''Get the closet prediction to camera focal point'''
+        # TODO: instead of camera focal point; calibrate closet pred to operator view
+        H, W, C = rgb_img.shape
+        focal_y = H / 2
+        focal_x = W / 2
         closet_pred = None
         closet_dist = None # Cloest to camera in z-axis
         obj_of_interest = [f"armor_{enemy_team}"]
+        closet_dist = 99999999
         for name, conf, bbox in pred_list:
             if name not in obj_of_interest: continue
-            cur_dist = Utils.estimate_target_depth(bbox, depth_map)
+            center_x, center_y, width, height = bbox
+            cur_dist = (center_x - focal_x)**2 + (center_y - focal_y)**2
             if closet_pred is None:
                 closet_pred = (name, conf, bbox)
                 closet_dist = cur_dist
@@ -71,7 +78,7 @@ def get_closet_pred(self, pred_list, enemy_team, depth_map):
 
     @staticmethod
     def get_rotation_angle(bbox_center_x, bbox_center_y):
-        yaw_diff = (bbox_center_x - config.IMG_CENTER_X) * (config.RGBD_CAMERA.YAW_FOV_HALF / config.IMG_CENTER_X)
-        pitch_diff = (bbox_center_y - config.IMG_CENTER_Y) * (config.RGBD_CAMERA.PITCH_FOV_HALF / config.IMG_CENTER_Y)
+        yaw_diff = (bbox_center_x - config.IMG_CENTER_X) * (config.AUTOAIM_CAMERA.YAW_FOV_HALF / config.IMG_CENTER_X)
+        pitch_diff = (bbox_center_y - config.IMG_CENTER_Y) * (config.AUTOAIM_CAMERA.PITCH_FOV_HALF / config.IMG_CENTER_Y)
 
         return yaw_diff, pitch_diff
diff --git a/Aiming/Tracking/basic_tracker.py b/Aiming/Tracking/basic_tracker.py
index 4097a43..35b08c8 100644
--- a/Aiming/Tracking/basic_tracker.py
+++ b/Aiming/Tracking/basic_tracker.py
@@ -11,7 +11,7 @@ def __init__(self):
         self.last_pred = None
         self.last_last_pred = None
 
-    def register_one(self, pred_list, enemy_team, rgb_img, depth_map):
+    def register_one(self, pred_list, enemy_team, rgb_img):
         # TODO: add a tracking flag in pred_list to distinguish
         # tracked and detected objects
         my_pred_list = [i for i in pred_list if i[0] == f"armor_{enemy_team}"]
diff --git a/Camera/d455.py b/Camera/d455.py.deprecated
similarity index 100%
rename from Camera/d455.py
rename to Camera/d455.py.deprecated
diff --git a/Camera/read_from_file.py b/Camera/read_from_file.py
index 1743e46..acb39db 100644
--- a/Camera/read_from_file.py
+++ b/Camera/read_from_file.py
@@ -33,8 +33,8 @@ def get_frame(self):
             self.alive = False
         
         if not self.alive:
-            raise
+            raise Exception("Video file exhausted")
         
         frame = cv2.resize(frame, (self.width, self.height))
 
-        return (frame, np.ones_like(frame))
+        return frame
diff --git a/Utils/__init__.py b/Utils/__init__.py
index 918f60f..183c7da 100644
--- a/Utils/__init__.py
+++ b/Utils/__init__.py
@@ -6,27 +6,3 @@ def deg_to_rad(deg):
 
 def rad_to_deg(rad):
     return rad * (360. / (2 * np.pi))
-
-def estimate_target_depth(bbox, depth_map):
-    center_x, center_y, width, height = bbox
-
-    upper_left_x = int(center_x - width / 2)
-    upper_left_y = int(center_y - height / 2)
-    lower_right_x = int(center_x + width / 2)
-    lower_right_y = int(center_y + height / 2)
-
-    # Get distance to target
-    depth_region = depth_map[upper_left_y:lower_right_y,upper_left_x:lower_right_x]
-    estimated_depth = np.mean(depth_region[depth_region > 0])
-
-    if np.isnan(estimated_depth):
-        # if NaN (i.e., all depth observations are invalid), use bbox size for a rough estimation
-        # intentionally scale to be 'virtually' further than estimated depth
-        # so that
-        #       1. samples with valid depth estimation are preferred
-        #       2. samples with larger bbox are preferred
-        estimated_depth = 100000 + (100. / (width * height))
-    else:
-        assert estimated_depth >= 0 and estimated_depth <= 2**16 - 1
-
-    return estimated_depth
diff --git a/config.py b/config.py
index d4dbc2b..b13bf27 100644
--- a/config.py
+++ b/config.py
@@ -12,7 +12,7 @@
 # from Camera.d455 import D455_camera
 from Camera.read_from_file import fake_camera
 
-RGBD_CAMERA = fake_camera
+AUTOAIM_CAMERA = fake_camera
 
 # Compute some constants and define camera to use
 IMG_HEIGHT=360
diff --git a/vision.py b/vision.py
index 612a4e3..6feee08 100644
--- a/vision.py
+++ b/vision.py
@@ -49,7 +49,7 @@ def get_enemy_color(self):
     communicator = serial_port
     pkt_seq = 0
 
-    rgbd_camera = config.RGBD_CAMERA(config.IMG_WIDTH, config.IMG_HEIGHT)
+    autoaim_camera = config.AUTOAIM_CAMERA(config.IMG_WIDTH, config.IMG_HEIGHT)
 
     # color buffer which retrieves enemy color from STM32
     my_color_buffer = serial_circular_buffer()
@@ -59,7 +59,7 @@ def get_enemy_color(self):
 
     while True:
         start = time.time()
-        frame, depth = rgbd_camera.get_frame()
+        frame = autoaim_camera.get_frame()
 
         if communicator is not None:
             if (communicator.inWaiting() > 0):
@@ -84,7 +84,7 @@ def get_enemy_color(self):
         elapsed = time.time()-start
         
         # Tracking and filtering
-        ret_dict = aimer.process_one(pred, enemy_team, frame, depth)
+        ret_dict = aimer.process_one(pred, enemy_team, frame)
 
         show_frame = frame.copy()
 

From ce7a9e04936191b8ff9affaab8980527cc1db495 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sat, 28 Jan 2023 02:28:29 -0600
Subject: [PATCH 08/25] update .gitignore

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.gitignore b/.gitignore
index b6e4761..c821c84 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,5 @@
+.DS_Store
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

From b4ad33f6e8c01c5236e2360d8d78dbc697de9ae1 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sat, 28 Jan 2023 02:28:45 -0600
Subject: [PATCH 09/25] update README

---
 README.md | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/README.md b/README.md
index 3be3e94..c351845 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,5 @@
-# iRM_Vision_2022
+# iRM_Vision_2023
+
 repo for iRM vision.
 
 ## Data preparing
@@ -26,9 +27,3 @@ Please follow instruction from the RMCV101 repo [here](https://github.com/illini
 - config.py            --> global config (parameters and class instance)
 ```
 
-## Dependencies:
-- [Darknet](https://github.com/pjreddie/darknet)
-  - OpenCV
-  - CUDA
-- Python 
-

From ac490c30afe0194917a0fb171289bdbb1b165aa2 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 5 Feb 2023 17:36:55 -0600
Subject: [PATCH 10/25] fix variable names in communicator; default conf for
 armor

---
 Communication/communicator.py | 4 ++--
 Detection/CV_mix_DL.py        | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/Communication/communicator.py b/Communication/communicator.py
index 4a36852..e816684 100644
--- a/Communication/communicator.py
+++ b/Communication/communicator.py
@@ -38,8 +38,8 @@ def create_packet(header, seq_num, yaw_offset, pitch_offset):
     assert seq_num >= 0 and seq_num < 2**32 - 1 # uint32
     packet += seq_num.to_bytes(4, 'big')
     # YAW/PITCH offset should not be too high
-    assert yaw_offset >= -config.RGBD_CAMERA.YAW_FOV_HALF and yaw_offset <= config.RGBD_CAMERA.YAW_FOV_HALF
-    assert pitch_offset >= -config.RGBD_CAMERA.PITCH_FOV_HALF and pitch_offset <= config.RGBD_CAMERA.PITCH_FOV_HALF
+    assert yaw_offset >= -config.AUTOAIM_CAMERA.YAW_FOV_HALF and yaw_offset <= config.AUTOAIM_CAMERA.YAW_FOV_HALF
+    assert pitch_offset >= -config.AUTOAIM_CAMERA.PITCH_FOV_HALF and pitch_offset <= config.AUTOAIM_CAMERA.PITCH_FOV_HALF
     discrete_yaw_offset = int(yaw_offset * 100000)
     discrete_pitch_offset = int(pitch_offset * 100000)
     packet += (discrete_yaw_offset & 0xFFFFFFFF).to_bytes(4, 'big')
diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 2214b0a..147d372 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -110,6 +110,7 @@ def __init__(self, light1, light2, color):
         assert light1.color == light2.color
         assert light1.color == color
         self.color = color
+        self.confidence = 0.0
         
         if light1.center_x < light2.center_x:
             self.left_light = light1

From 7cedf860ec9d2741efc95073401e193789fb468a Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 5 Feb 2023 19:24:15 -0600
Subject: [PATCH 11/25] move DEBUG_DISPLAY flag to config.py

---
 config.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/config.py b/config.py
index b13bf27..ad4a3c4 100644
--- a/config.py
+++ b/config.py
@@ -20,3 +20,5 @@
 
 IMG_CENTER_X = IMG_WIDTH // 2
 IMG_CENTER_Y = IMG_HEIGHT // 2
+
+DEBUG_DISPLAY = False

From 3e9a926c55671b944ad961f90fb49c163a35b292 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 5 Feb 2023 19:25:15 -0600
Subject: [PATCH 12/25] add visualization for all detected armors

---
 vision.py | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/vision.py b/vision.py
index 6feee08..a4133b5 100644
--- a/vision.py
+++ b/vision.py
@@ -8,8 +8,6 @@
 
 DEFAULT_ENEMY_TEAM = 'red'
 
-DEBUG_DISPLAY = True
-
 class serial_circular_buffer:
     def __init__(self, buffer_size=10):
         self.buffer = []
@@ -82,6 +80,18 @@ def get_enemy_color(self):
             pred[i] = (name_str, conf, bbox)
 
         elapsed = time.time()-start
+
+        if config.DEBUG_DISPLAY:
+            viz_frame = frame.copy()
+            for name, conf, bbox in pred:
+                lower_x = int(bbox[0] - bbox[2] / 2)
+                lower_y = int(bbox[1] - bbox[3] / 2)
+                upper_x = int(bbox[0] + bbox[2] / 2)
+                upper_y = int(bbox[1] + bbox[3] / 2)
+                viz_frame = cv2.rectangle(viz_frame, (lower_x, lower_y), (upper_x, upper_y), (0, 255, 0), 2)
+            cv2.imshow('all_detected', viz_frame)
+            if cv2.waitKey(1) & 0xFF == ord('q'):
+                exit(0)
         
         # Tracking and filtering
         ret_dict = aimer.process_one(pred, enemy_team, frame)
@@ -96,12 +106,12 @@ def get_enemy_color(self):
         else:
             show_frame = cv2.putText(show_frame, 'NOT FOUND', (50, 50),
                                     cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
-            packet = create_packet(config.SEARCH_TARGET, pkt_seq, 0, 0)
+            packet = create_packet(config.SEARCH_TARGET, pkt_seq, 0, 0)g
         
-        if DEBUG_DISPLAY:
+        if config.DEBUG_DISPLAY:
             print('----------------\n',pred)
             print('fps:',1./elapsed)
-            cv2.imshow('pred', show_frame)
+            cv2.imshow('target', show_frame)
             if cv2.waitKey(1) & 0xFF == ord('q'):
                 exit(0)
 

From b74d4aa3507c5fe047933793eec047778ce9bd18 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 5 Feb 2023 19:56:09 -0600
Subject: [PATCH 13/25] use individual luminance for more robust detection but
 w/ reduced FPS

---
 Aiming/Aim.py          |  6 ++--
 Detection/CV_mix_DL.py | 65 +++++++++++++++++++++++++++++++++++++-----
 vision.py              |  2 +-
 3 files changed, 63 insertions(+), 10 deletions(-)

diff --git a/Aiming/Aim.py b/Aiming/Aim.py
index 374354e..ea10ecb 100644
--- a/Aiming/Aim.py
+++ b/Aiming/Aim.py
@@ -11,9 +11,11 @@ def __init__(self):
     def process_one(self, pred_list, enemy_team, rgb_img):
         assert enemy_team in ['blue', 'red']
 
-        pred_list = self.tracker.fix_prediction(pred_list)
+        # FIXME: tracker is commented out for now because it works only for one target
+        # TODO: add HUNGARIAN algorithm to match predictions in tracker!
+        # pred_list = self.tracker.fix_prediction(pred_list)
 
-        self.tracker.register_one(pred_list, enemy_team, rgb_img)
+        # self.tracker.register_one(pred_list, enemy_team, rgb_img)
 
         closet_pred, closet_dist = self.get_closet_pred(pred_list, enemy_team, rgb_img)
         if closet_pred is None:
diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 147d372..15f7e67 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -3,6 +3,10 @@
 import os
 import time
 
+# Internal ENUM
+RED = 0
+BLUE = 1
+
 def auto_align_brightness(img, target_v=50):
     # Only decrease!
     hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
@@ -30,9 +34,10 @@ def auto_align_brightness(img, target_v=50):
         return img
 
 class cv_mix_dl_detector:
-    def __init__(self, detect_color, model_path='fc.onnx'):
-        self.armor_proposer = cv_armor_proposer(detect_color)
-        self.digit_classifier = dl_digit_classifier(model_path)
+    def __init__(self, config, detect_color, model_path='fc.onnx'):
+        self.CFG = config
+        self.armor_proposer = cv_armor_proposer(self.CFG, detect_color)
+        self.digit_classifier = dl_digit_classifier(self.CFG, model_path)
         self.change_color(detect_color)
     
     def detect(self, rgb_img):
@@ -159,14 +164,17 @@ def extract_number(self, rgb_img):
         self.number_image = cv2.cvtColor(self.number_image, cv2.COLOR_RGB2GRAY)
         thres, self.number_image = cv2.threshold(self.number_image, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
 
-class cv_armor_proposer:
+class cv_armor_proposer(object):
     # Hyperparameters
     MIN_LIGHTNESS = 160
+    LUMINANCE_THRES = 90
 
     LIGHT_MIN_RATIO = 0.1
     LIGHT_MAX_RATIO = 0.55
     LIGHT_MAX_ANGLE = 40.0
 
+    LIGHT_AREA_THRES = 50
+
     ARMOR_MIN_LIGHT_RATIO = 0.6
     ARMOR_MIN_SMALL_CENTER_DISTANCE = 1.3
     ARMOR_MAX_SMALL_CENTER_DISTANCE = 4
@@ -177,13 +185,34 @@ class cv_armor_proposer:
 
     armor_max_angle = 35.0
 
-    def __init__(self, detect_color):
+    def __init__(self, config, detect_color):
+        self.CFG = config
         self.detect_color = detect_color
     
     def __call__(self, rgb_img):
         binary_img = self.preprocess(rgb_img)
+        if self.CFG.DEBUG_DISPLAY:
+            # visualize binary image
+            cv2.imshow('binary', binary_img)
+            cv2.waitKey(1)
+
         light_list = self.find_lights(rgb_img, binary_img)
+        if self.CFG.DEBUG_DISPLAY:
+            viz_img = rgb_img.copy()
+            # visualize lights
+            for light in light_list:
+                cv2.rectangle(viz_img, (int(light.top[0]), int(light.top[1])), (int(light.btm[0]), int(light.btm[1])), (0, 255, 0), 2)
+            cv2.imshow('lights', viz_img)
+            cv2.waitKey(1)
+
         armor_list = self.match_lights(light_list)
+        if self.CFG.DEBUG_DISPLAY:
+            viz_img = rgb_img.copy()
+            # visualize armors
+            for armor in armor_list:
+                cv2.rectangle(viz_img, (int(armor.left_light.top[0]), int(armor.left_light.top[1])), (int(armor.right_light.btm[0]), int(armor.right_light.btm[1])), (0, 255, 0), 2)
+            cv2.imshow('armors', viz_img)
+            cv2.waitKey(1)
         
         return armor_list
     
@@ -198,7 +227,25 @@ def preprocess(self, rgb_img):
         return binary_img
 
     def find_lights(self, rgb_img, binary_img):
-        contours, hierarchy = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        # Use difference of color to handle white light
+        # TODO: move these logics to preprocess
+        if self.detect_color == RED:
+            color_diff = rgb_img[:,:,0].astype(int) - rgb_img[:,:,2]
+            color_diff[color_diff < 0] = 0
+            color_diff = color_diff.astype(np.uint8)
+            _, color_bin = cv2.threshold(color_diff, self.LUMINANCE_THRES, 255, cv2.THRESH_BINARY)
+        else:
+            color_diff = rgb_img[:,:,2].astype(int) - rgb_img[:,:,0]
+            color_diff[color_diff < 0] = 0
+            color_diff = color_diff.astype(np.uint8)
+            _, color_bin = cv2.threshold(color_diff, self.LUMINANCE_THRES, 255, cv2.THRESH_BINARY)
+        
+        cv2.imshow('color', color_bin)
+        cv2.waitKey(1)
+
+        combined_img = cv2.bitwise_or(binary_img, color_bin)
+        contours, _ = cv2.findContours(combined_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
         light_list = []
         start_cp = time.time()
         for contour in contours:
@@ -214,6 +261,9 @@ def find_lights(self, rgb_img, binary_img):
 
             x, y, w, h = bounding_rect
 
+            if w * h < self.LIGHT_AREA_THRES:
+                continue
+
             if 0 <= x and 0 <= w and (x + w) <= rgb_img.shape[1] and 0 < y and 0 < h and y + h <=rgb_img.shape[0]:
                 sum_r = 0
                 sum_b = 0
@@ -314,7 +364,8 @@ class dl_digit_classifier:
     LABEL_NAMES_LIST = np.array(['B', '1', '2', '3', '4', '5', 'G', 'O', 'N'])
     CLASSIFIER_THRESHOLD = 0.7
 
-    def __init__(self, model_path):
+    def __init__(self, config, model_path):
+        self.CFG = config
         self.net = cv2.dnn.readNetFromONNX(model_path)
     
     @staticmethod
diff --git a/vision.py b/vision.py
index a4133b5..660955f 100644
--- a/vision.py
+++ b/vision.py
@@ -41,7 +41,7 @@ def get_enemy_color(self):
         return self.default_color
 
 if __name__ == "__main__":
-    model = cv_mix_dl_detector(DEFAULT_ENEMY_TEAM)
+    model = cv_mix_dl_detector(config, DEFAULT_ENEMY_TEAM)
     # model = Yolo(config.MODEL_CFG_PATH, config.WEIGHT_PATH, config.META_PATH)
     aimer = Aim()
     communicator = serial_port

From 103a20ab0c74ba1a9d761d1bf3c36ae664d0d412 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sun, 5 Feb 2023 20:01:43 -0600
Subject: [PATCH 14/25] remove accidental typo & missed DEBUG_DISPLAY

---
 Detection/CV_mix_DL.py | 7 ++++---
 config.py              | 2 +-
 vision.py              | 2 +-
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 15f7e67..e8f1950 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -228,7 +228,7 @@ def preprocess(self, rgb_img):
 
     def find_lights(self, rgb_img, binary_img):
         # Use difference of color to handle white light
-        # TODO: move these logics to preprocess
+        # TODO separate contours computation and use NMS w/ light filtering
         if self.detect_color == RED:
             color_diff = rgb_img[:,:,0].astype(int) - rgb_img[:,:,2]
             color_diff[color_diff < 0] = 0
@@ -240,8 +240,9 @@ def find_lights(self, rgb_img, binary_img):
             color_diff = color_diff.astype(np.uint8)
             _, color_bin = cv2.threshold(color_diff, self.LUMINANCE_THRES, 255, cv2.THRESH_BINARY)
         
-        cv2.imshow('color', color_bin)
-        cv2.waitKey(1)
+        if self.CFG.DEBUG_DISPLAY:
+            cv2.imshow('color', color_bin)
+            cv2.waitKey(1)
 
         combined_img = cv2.bitwise_or(binary_img, color_bin)
         contours, _ = cv2.findContours(combined_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
diff --git a/config.py b/config.py
index ad4a3c4..7ae8f1b 100644
--- a/config.py
+++ b/config.py
@@ -21,4 +21,4 @@
 IMG_CENTER_X = IMG_WIDTH // 2
 IMG_CENTER_Y = IMG_HEIGHT // 2
 
-DEBUG_DISPLAY = False
+DEBUG_DISPLAY = True
diff --git a/vision.py b/vision.py
index 660955f..56b4426 100644
--- a/vision.py
+++ b/vision.py
@@ -106,7 +106,7 @@ def get_enemy_color(self):
         else:
             show_frame = cv2.putText(show_frame, 'NOT FOUND', (50, 50),
                                     cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
-            packet = create_packet(config.SEARCH_TARGET, pkt_seq, 0, 0)g
+            packet = create_packet(config.SEARCH_TARGET, pkt_seq, 0, 0)
         
         if config.DEBUG_DISPLAY:
             print('----------------\n',pred)

From 9d8bcb94daed916b508582131c0a6c905df84a47 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Mon, 6 Feb 2023 23:55:21 -0600
Subject: [PATCH 15/25] add draft for using NMS for more robust detection

---
 Detection/CV_mix_DL.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index e8f1950..92eeee2 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -234,16 +234,19 @@ def find_lights(self, rgb_img, binary_img):
             color_diff[color_diff < 0] = 0
             color_diff = color_diff.astype(np.uint8)
             _, color_bin = cv2.threshold(color_diff, self.LUMINANCE_THRES, 255, cv2.THRESH_BINARY)
+            color_contours, _ = cv2.findContours(color_bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
         else:
             color_diff = rgb_img[:,:,2].astype(int) - rgb_img[:,:,0]
             color_diff[color_diff < 0] = 0
             color_diff = color_diff.astype(np.uint8)
             _, color_bin = cv2.threshold(color_diff, self.LUMINANCE_THRES, 255, cv2.THRESH_BINARY)
-        
+            color_contours, _ = cv2.findContours(color_bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    
         if self.CFG.DEBUG_DISPLAY:
             cv2.imshow('color', color_bin)
             cv2.waitKey(1)
 
+        # TODO: use NMS here
         combined_img = cv2.bitwise_or(binary_img, color_bin)
         contours, _ = cv2.findContours(combined_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
 

From c7a7464c95f10a181e79681653c54223820fdd01 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Thu, 9 Feb 2023 11:54:59 -0600
Subject: [PATCH 16/25] use NMS-like filtering for more robust detection

---
 Detection/CV_mix_DL.py | 41 ++++++++++++++++++++++++++++++++---------
 1 file changed, 32 insertions(+), 9 deletions(-)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 92eeee2..aa10ab7 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -2,7 +2,7 @@
 import cv2
 import os
 import time
-
+from IPython import embed
 # Internal ENUM
 RED = 0
 BLUE = 1
@@ -227,6 +227,7 @@ def preprocess(self, rgb_img):
         return binary_img
 
     def find_lights(self, rgb_img, binary_img):
+        bin_contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
         # Use difference of color to handle white light
         # TODO separate contours computation and use NMS w/ light filtering
         if self.detect_color == RED:
@@ -245,17 +246,39 @@ def find_lights(self, rgb_img, binary_img):
         if self.CFG.DEBUG_DISPLAY:
             cv2.imshow('color', color_bin)
             cv2.waitKey(1)
-
-        # TODO: use NMS here
-        combined_img = cv2.bitwise_or(binary_img, color_bin)
-        contours, _ = cv2.findContours(combined_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        
+        # Preprocess contours for early filtering
+        bin_contours = [contour for contour in bin_contours if contour.shape[0] >= 5]
+        color_contours = [contour for contour in color_contours if contour.shape[0] >= 5]
+
+        # TODO: apply color test here
+
+        # Apply NMS to contours from binary image and color difference image
+        # COLOR contours are less reliable
+        bin_rects = [cv2.boundingRect(contour) for contour in bin_contours]
+        col_rects = [cv2.boundingRect(contour) for contour in color_contours]
+        final_contours = []
+        for col_rect, col_contour in zip(col_rects, color_contours):
+            append_flag = True
+            for bin_rect in bin_rects:
+                # Compute rectangle overlap
+                x_overlap = max(0, min(bin_rect[0] + bin_rect[2], col_rect[0] + col_rect[2]) - max(bin_rect[0], col_rect[0]))
+                y_overlap = max(0, min(bin_rect[1] + bin_rect[3], col_rect[1] + col_rect[3]) - max(bin_rect[1], col_rect[1]))
+                overlap = x_overlap * y_overlap
+                min_area = min(bin_rect[2] * bin_rect[3], col_rect[2] * col_rect[3])
+                normalized_overlap = overlap / min_area
+                NMS_THRES = 0.5
+                if normalized_overlap > NMS_THRES:
+                    # If overlap is too large, discard color contour
+                    append_flag = False
+                    break
+            if append_flag:
+                final_contours.append(col_contour)
+        final_contours.extend(bin_contours)
 
         light_list = []
         start_cp = time.time()
-        for contour in contours:
-            if contour.shape[0] < 5:
-                continue
-
+        for contour in final_contours:
             light = cv2.minAreaRect(contour)
             light = light_class(light)
             if not self.is_light(light):

From f9163699463cf6729ab29fd1c24c3f66fe6ea414 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Thu, 9 Feb 2023 12:11:09 -0600
Subject: [PATCH 17/25] implement frame counter and timer in fake_camera

---
 Camera/read_from_file.py | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/Camera/read_from_file.py b/Camera/read_from_file.py
index acb39db..7ccb0fc 100644
--- a/Camera/read_from_file.py
+++ b/Camera/read_from_file.py
@@ -1,5 +1,6 @@
 # Read from file
 import sys
+import time
 import numpy as np
 import cv2
 import Utils
@@ -23,7 +24,14 @@ def __init__(self, width, height):
 
         self.alive = True # init to True
 
+        # Timing and frame counter are always in place for devlopment purpose
+        self.timing = None
+        self.frame_cnt = 0
+
     def get_frame(self):
+        if self.timing is None:
+            self.timing = time.time()
+
         if not self.cap.isOpened():
             self.alive = False
         
@@ -33,8 +41,13 @@ def get_frame(self):
             self.alive = False
         
         if not self.alive:
+            print("Total frames: {}".format(self.frame_cnt))
+            print("Total time: {}".format(time.time() - self.timing))
+            print("FPS: {}".format(self.frame_cnt / (time.time() - self.timing)))
             raise Exception("Video file exhausted")
         
         frame = cv2.resize(frame, (self.width, self.height))
 
+        self.frame_cnt += 1
+
         return frame

From 3b61cca7be31c0826b49d34f1a791c3b8409a513 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Thu, 9 Feb 2023 12:12:30 -0600
Subject: [PATCH 18/25] FPS: 117->147 by using approximated polygon test

---
 Detection/CV_mix_DL.py | 41 ++++++++++++++++++-----------------------
 config.py              |  2 +-
 2 files changed, 19 insertions(+), 24 deletions(-)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index aa10ab7..b31a619 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -33,6 +33,15 @@ def auto_align_brightness(img, target_v=50):
         img = cv2.cvtColor(final_hsv, cv2.COLOR_HSV2BGR)
         return img
 
+def color_test(rgb_img, rect, color):
+    rgb_roi = rgb_img[rect[1]:rect[1]+rect[3], rect[0]:rect[0]+rect[2]]
+    sum_r = np.sum(rgb_roi[:,:,0])
+    sum_b = np.sum(rgb_roi[:,:,2])
+    if color == RED:
+        return sum_r >= sum_b
+    else:
+        return sum_b >= sum_r
+
 class cv_mix_dl_detector:
     def __init__(self, config, detect_color, model_path='fc.onnx'):
         self.CFG = config
@@ -251,12 +260,15 @@ def find_lights(self, rgb_img, binary_img):
         bin_contours = [contour for contour in bin_contours if contour.shape[0] >= 5]
         color_contours = [contour for contour in color_contours if contour.shape[0] >= 5]
 
-        # TODO: apply color test here
-
-        # Apply NMS to contours from binary image and color difference image
-        # COLOR contours are less reliable
         bin_rects = [cv2.boundingRect(contour) for contour in bin_contours]
         col_rects = [cv2.boundingRect(contour) for contour in color_contours]
+
+        # Apply color test here
+        bin_rects = [r for r in bin_rects if color_test(rgb_img, r, self.detect_color)]
+        col_rects = [r for r in col_rects if color_test(rgb_img, r, self.detect_color)]
+
+        # Apply NMS to contours from binary image and color difference image
+        # COLOR contours are generally less reliable
         final_contours = []
         for col_rect, col_contour in zip(col_rects, color_contours):
             append_flag = True
@@ -291,25 +303,8 @@ def find_lights(self, rgb_img, binary_img):
             if w * h < self.LIGHT_AREA_THRES:
                 continue
 
-            if 0 <= x and 0 <= w and (x + w) <= rgb_img.shape[1] and 0 < y and 0 < h and y + h <=rgb_img.shape[0]:
-                sum_r = 0
-                sum_b = 0
-
-                roi = rgb_img[y:y+h,x:x+w]
-
-                for i in range(roi.shape[0]):
-                    for j in range(roi.shape[1]):
-                        if cv2.pointPolygonTest(contour, (j + x, i + y), False):
-                            # point is inside contour
-                            sum_r += roi[i,j,0]
-                            sum_b += roi[i,j,2]
-
-                if sum_r > sum_b:
-                    my_color = 0 # RED
-                else:
-                    my_color = 1 # BLUE
-                light.color = my_color
-                light_list.append(light)
+            light.color = self.detect_color
+            light_list.append(light)
         return light_list
     
     def is_light(self, light):
diff --git a/config.py b/config.py
index 7ae8f1b..ad4a3c4 100644
--- a/config.py
+++ b/config.py
@@ -21,4 +21,4 @@
 IMG_CENTER_X = IMG_WIDTH // 2
 IMG_CENTER_Y = IMG_HEIGHT // 2
 
-DEBUG_DISPLAY = True
+DEBUG_DISPLAY = False

From d3d9585f698305b846c2729a59110e9c5c7a9afd Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Thu, 9 Feb 2023 12:23:28 -0600
Subject: [PATCH 19/25] FPS: 147->151 by doing early area thresholding

---
 Detection/CV_mix_DL.py | 41 ++++++++++++++++++++++++++++-------------
 1 file changed, 28 insertions(+), 13 deletions(-)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index b31a619..542f9f8 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -234,6 +234,28 @@ def preprocess(self, rgb_img):
         thres, binary_img = cv2.threshold(gray_img, self.MIN_LIGHTNESS, 255, cv2.THRESH_BINARY)
 
         return binary_img
+    
+    def filter_contours_rects(self, contours, rects, rgb_img):
+        """
+        Filter out contours that are too small or too large
+        """
+        filtered_contours = []
+        filtered_rects = []
+        assert len(contours) == len(rects)
+        for i in range(len(rects)):
+            x, y, w, h = rects[i]
+            # Area test
+            if w * h < self.LIGHT_AREA_THRES:
+                continue
+            
+            # Color test
+            if not color_test(rgb_img, rects[i], self.detect_color):
+                continue
+
+            filtered_rects.append(rects[i])
+            filtered_contours.append(contours[i])
+        
+        return filtered_contours, filtered_rects
 
     def find_lights(self, rgb_img, binary_img):
         bin_contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
@@ -263,9 +285,9 @@ def find_lights(self, rgb_img, binary_img):
         bin_rects = [cv2.boundingRect(contour) for contour in bin_contours]
         col_rects = [cv2.boundingRect(contour) for contour in color_contours]
 
-        # Apply color test here
-        bin_rects = [r for r in bin_rects if color_test(rgb_img, r, self.detect_color)]
-        col_rects = [r for r in col_rects if color_test(rgb_img, r, self.detect_color)]
+        # Basic filtering for contours and rects
+        bin_contours, bin_rects = self.filter_contours_rects(bin_contours, bin_rects, rgb_img)
+        color_contours, col_rects = self.filter_contours_rects(color_contours, col_rects, rgb_img)
 
         # Apply NMS to contours from binary image and color difference image
         # COLOR contours are generally less reliable
@@ -289,18 +311,11 @@ def find_lights(self, rgb_img, binary_img):
         final_contours.extend(bin_contours)
 
         light_list = []
-        start_cp = time.time()
         for contour in final_contours:
             light = cv2.minAreaRect(contour)
             light = light_class(light)
-            if not self.is_light(light):
-                continue
-
-            bounding_rect = cv2.boundingRect(contour)
 
-            x, y, w, h = bounding_rect
-
-            if w * h < self.LIGHT_AREA_THRES:
+            if not self.is_light(light):
                 continue
 
             light.color = self.detect_color
@@ -323,8 +338,8 @@ def match_lights(self, light_list):
                 light1 = light_list[i]
                 light2 = light_list[j]
 
-                if light1.color != self.detect_color or light2.color != self.detect_color:
-                    continue
+                assert light1.color == self.detect_color
+                assert light2.color == self.detect_color
 
                 if self.contain_light(light1, light2, light_list):
                     continue

From 89e5cd1325dfd77da9571ccbe04a6c20be257c86 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sat, 11 Feb 2023 16:24:27 -0600
Subject: [PATCH 20/25] fixed broken README http link

---
 Detection/CV_mix_DL.py | 1 -
 README.md              | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 542f9f8..9da41ff 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -260,7 +260,6 @@ def filter_contours_rects(self, contours, rects, rgb_img):
     def find_lights(self, rgb_img, binary_img):
         bin_contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
         # Use difference of color to handle white light
-        # TODO separate contours computation and use NMS w/ light filtering
         if self.detect_color == RED:
             color_diff = rgb_img[:,:,0].astype(int) - rgb_img[:,:,2]
             color_diff[color_diff < 0] = 0
diff --git a/README.md b/README.md
index c351845..121f41e 100644
--- a/README.md
+++ b/README.md
@@ -8,7 +8,7 @@ We recorded some demo videos to demo auto-aiming capabilities of our robots,
 which is located under the folder `./large_data/`.
 However, due to the large sizes of the video, it's inappropriate to directly
 upload them to GitHub. Hence, to acquire theese sample videos, please download
-them at the [UofI box][https://uofi.box.com/s/i6zahotr9id35hurjzy2bq3dcfz0085e].
+them at the [UofI box](https://uofi.box.com/s/i6zahotr9id35hurjzy2bq3dcfz0085e).
 
 ## Dependencies
 

From 7f028f89537903299e153bba33b32608fcff17cf Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Sat, 11 Feb 2023 18:53:09 -0600
Subject: [PATCH 21/25] implement and tested Hungarian matching for tracking
 assocation + visualization

---
 Aiming/Aim.py                        |  30 +++----
 Aiming/Tracking/basic_tracker.py     | 122 +++++++++++++++++++++------
 Aiming/Tracking/consistent_id_gen.py |  17 ++++
 Detection/CV_mix_DL.py               |   1 +
 config.py                            |   2 +-
 vision.py                            |  24 +++++-
 6 files changed, 150 insertions(+), 46 deletions(-)
 create mode 100644 Aiming/Tracking/consistent_id_gen.py

diff --git a/Aiming/Aim.py b/Aiming/Aim.py
index ea10ecb..ae981cc 100644
--- a/Aiming/Aim.py
+++ b/Aiming/Aim.py
@@ -5,23 +5,23 @@
 from .Tracking import basic_tracker
 
 class Aim:
-    def __init__(self):
-        self.tracker = basic_tracker()
+    def __init__(self, config):
+        self.CFG = config
+        self.tracker = basic_tracker(self.CFG)
 
     def process_one(self, pred_list, enemy_team, rgb_img):
         assert enemy_team in ['blue', 'red']
 
-        # FIXME: tracker is commented out for now because it works only for one target
-        # TODO: add HUNGARIAN algorithm to match predictions in tracker!
-        # pred_list = self.tracker.fix_prediction(pred_list)
+        # TODO: use assertion to check enemy_team
 
-        # self.tracker.register_one(pred_list, enemy_team, rgb_img)
+        final_bbox_list, final_id_list = self.tracker.process_one(pred_list, enemy_team, rgb_img)
+
+        # TODO: integrate this into tracking for consistent tracking
+        closet_pred, closet_dist = self.get_closet_pred(final_bbox_list, rgb_img)
 
-        closet_pred, closet_dist = self.get_closet_pred(pred_list, enemy_team, rgb_img)
         if closet_pred is None:
             return None
-        name, confidence, bbox = closet_pred
-        center_x, center_y, width, height = bbox
+        center_x, center_y, width, height = closet_pred
 
         # Get yaw/pitch differences in radians
         yaw_diff, pitch_diff = self.get_rotation_angle(center_x, center_y)
@@ -33,6 +33,8 @@ def process_one(self, pred_list, enemy_team, rgb_img):
             'pitch_diff': calibrated_pitch_diff,
             'center_x': center_x,
             'center_y': center_y,
+            'final_bbox_list': final_bbox_list,
+            'final_id_list': final_id_list,
         }
     
     def posterior_calibration(self, yaw_diff, pitch_diff, distance):
@@ -55,7 +57,7 @@ def posterior_calibration(self, yaw_diff, pitch_diff, distance):
             # TODO: compute a range table
             return (yaw_diff, pitch_diff)
     
-    def get_closet_pred(self, pred_list, enemy_team, rgb_img):
+    def get_closet_pred(self, bbox_list, rgb_img):
         '''Get the closet prediction to camera focal point'''
         # TODO: instead of camera focal point; calibrate closet pred to operator view
         H, W, C = rgb_img.shape
@@ -63,18 +65,16 @@ def get_closet_pred(self, pred_list, enemy_team, rgb_img):
         focal_x = W / 2
         closet_pred = None
         closet_dist = None # Cloest to camera in z-axis
-        obj_of_interest = [f"armor_{enemy_team}"]
         closet_dist = 99999999
-        for name, conf, bbox in pred_list:
-            if name not in obj_of_interest: continue
+        for bbox in bbox_list:
             center_x, center_y, width, height = bbox
             cur_dist = (center_x - focal_x)**2 + (center_y - focal_y)**2
             if closet_pred is None:
-                closet_pred = (name, conf, bbox)
+                closet_pred = bbox
                 closet_dist = cur_dist
             else:
                 if cur_dist < closet_dist:
-                    closet_pred = (name, conf, bbox)
+                    closet_pred = bbox
                     closet_dist = cur_dist
         return closet_pred, closet_dist
 
diff --git a/Aiming/Tracking/basic_tracker.py b/Aiming/Tracking/basic_tracker.py
index 35b08c8..f1543a3 100644
--- a/Aiming/Tracking/basic_tracker.py
+++ b/Aiming/Tracking/basic_tracker.py
@@ -1,43 +1,109 @@
 import os
+import scipy.optimize
 import numpy as np
 
+from .consistent_id_gen import ConsistentIdGenerator
+
+from IPython import embed
+
+# TODO: move this to config
+FRAME_BUFFER_SIZE = 10
+
+class tracked_armor(object):
+    def __init__(self, bbox, roi, frame_tick, armor_id):
+        self.bbox_buffer = [bbox]
+        self.roi_buffer = [roi]
+        self.observed_frame_tick = [frame_tick]
+        self.armor_id = armor_id # unique ID
+    
+    def compute_cost(self, other_armor):
+        assert isinstance(other_armor, tracked_armor)
+        # TODO: use more sophisticated metrics (e.g., RGB) as cost function
+        c_x, c_y, w, h = self.bbox_buffer[-1]
+        o_c_x, o_c_y, o_w, o_h = other_armor.bbox_buffer[-1]
+        return np.square(c_x - o_c_x) + np.square(c_y - o_c_y)
+    
+    def update(self, other_armor, frame_tick):
+        # Only call if these two armors are matched
+        self.bbox_buffer.append(other_armor.bbox_buffer[-1])
+        self.roi_buffer.append(other_armor.roi_buffer[-1])
+        self.observed_frame_tick.append(frame_tick)
+
+        # Maintain each armor's buffer so that anything older than FRAME_BUFFER_SIZE is dropped
+        self.bbox_buffer = self.bbox_buffer[-FRAME_BUFFER_SIZE:]
+        self.roi_buffer = self.roi_buffer[-FRAME_BUFFER_SIZE:]
+    
+    def predict_bbox(self, cur_frame_tick):
+        if cur_frame_tick == self.observed_frame_tick[-1] or len(self.bbox_buffer) == 1:
+            return self.bbox_buffer[-1]
+        else:
+            # Linear extrapolation
+            c_x, c_y, w, h = self.bbox_buffer[-1]
+            o_c_x, o_c_y, o_w, o_h = self.bbox_buffer[-2]
+            delta_tick = self.observed_frame_tick[-1] - self.observed_frame_tick[-2]
+            new_delta_tick = cur_frame_tick - self.observed_frame_tick[-1]
+            delta_x = (c_x - o_c_x) * new_delta_tick / delta_tick
+            delta_y = (c_y - o_c_y) * new_delta_tick / delta_tick
+            return (int(c_x + delta_x), int(c_y + delta_y), w, h)
+
 class basic_tracker(object):
     '''
     Basic tracker that can handle only one target.
 
     It memorizes the state of last two predictions and do linear extrapolation
     '''
-    def __init__(self):
-        self.last_pred = None
-        self.last_last_pred = None
-
-    def register_one(self, pred_list, enemy_team, rgb_img):
-        # TODO: add a tracking flag in pred_list to distinguish
-        # tracked and detected objects
-        my_pred_list = [i for i in pred_list if i[0] == f"armor_{enemy_team}"]
+    SE_THRESHOLD = 1800 # (30, 30) pixels away
 
-        assert len(my_pred_list) <= 1, "Basic tracker does not support multiple targets"
+    def __init__(self, config):
+        self.CFG = config
+        self.active_armors = []
+        self.id_gen = ConsistentIdGenerator()
+        self.frame_tick = 0 # TODO: use timestamp may be a better idea
+    
+    def process_one(self, pred_list, enemy_team, rgb_img):
+        new_armors = []
+        for name, conf, bbox in pred_list:
+            c_x, c_y, w, h = bbox
+            lower_x = int(c_x - w / 2)
+            upper_x = int(c_x + w / 2)
+            lower_y = int(c_y - h / 2)
+            upper_y = int(c_y + h / 2)
+            roi = rgb_img[lower_y:upper_y,lower_x:upper_x]
+            new_armors.append(tracked_armor(bbox, roi, self.frame_tick, self.id_gen.get_id()))
 
-        self.last_last_pred = self.last_pred
-        self.last_pred = my_pred_list
+        if len(self.active_armors) > 0:
+            # Try to associate with current armors
+            cost_matrix = np.zeros((len(new_armors), len(self.active_armors)), dtype=float)
+            for i in range(len(new_armors)):
+                for j in range(len(self.active_armors)):
+                    cost_ij = new_armors[i].compute_cost(self.active_armors[j])
+                    cost_matrix[i, j] = cost_ij
+            row_ind, col_ind = scipy.optimize.linear_sum_assignment(cost_matrix)
 
-    def fix_prediction(self, cur_pred_list):
-        if len(cur_pred_list) == 0:
-            last_name, last_conf, last_bbox = self.last_pred[0]
-            last_last_name, last_last_conf, last_last_bbox = self.last_last_pred[0]
+            for i, j in zip(row_ind, col_ind):
+                if cost_matrix[i, j] < self.SE_THRESHOLD:
+                    assert new_armors[i] is not None
+                    self.active_armors[j].update(new_armors[i], self.frame_tick)
+                    new_armors[i] = None
+        
+        new_armors = [i for i in new_armors if i is not None]
 
-            assert last_name == last_last_name
+        # Maintain current buffer. If an armor has not been observed by 10 frames, it is dropped
+        self.active_armors = [i for i in self.active_armors
+                                if self.frame_tick - i.observed_frame_tick[-1] < FRAME_BUFFER_SIZE]
+        
+        # Unassociated armors are registered as new armors
+        for a in new_armors:
+            self.active_armors.append(a)
+        
+        # Create a list of bbox and unique IDs to return
+        ret_bbox_list = []
+        ret_id_list = []
+        for a in self.active_armors:
+            # If an armor is observed, directly use the bbox
+            ret_bbox_list.append(a.predict_bbox(self.frame_tick))
+            ret_id_list.append(a.armor_id)
 
-            last_center_x, last_center_y, last_width, last_height = last_bbox
-            last_last_center_x, last_last_center_y, last_last_width, last_last_height = last_last_bbox
+        self.frame_tick += 1
 
-            # Linear extrapolation
-            center_x = last_center_x + (last_center_x - last_last_center_x) / 2
-            center_y = last_center_y + (last_center_y - last_last_center_y) / 2
-            width = last_width + (last_width - last_last_width) / 2
-            height = last_height + (last_height - last_last_height) / 2
-
-            return [[last_name, (last_conf + last_last_conf) / 2, [center_x, center_y, width, height]]]
-        else:
-            # If not empty, do nothing
-            return cur_pred_list
+        return ret_bbox_list, ret_id_list
diff --git a/Aiming/Tracking/consistent_id_gen.py b/Aiming/Tracking/consistent_id_gen.py
new file mode 100644
index 0000000..3d8db48
--- /dev/null
+++ b/Aiming/Tracking/consistent_id_gen.py
@@ -0,0 +1,17 @@
+import threading
+
+class ConsistentIdGenerator(object):
+    def __init__(self, lock=False):
+        self.lock_flag = lock
+        if self.lock_flag:
+            self._lock = threading.Lock()
+        self._id = 0
+
+    def get_id(self):
+        if self.lock_flag:
+            with self._lock:
+                self._id += 1
+                return self._id
+        else:
+            self._id += 1
+            return self._id
diff --git a/Detection/CV_mix_DL.py b/Detection/CV_mix_DL.py
index 9da41ff..7f3b802 100644
--- a/Detection/CV_mix_DL.py
+++ b/Detection/CV_mix_DL.py
@@ -44,6 +44,7 @@ def color_test(rgb_img, rect, color):
 
 class cv_mix_dl_detector:
     def __init__(self, config, detect_color, model_path='fc.onnx'):
+        # TODO: fix config parameter in ipython notebook
         self.CFG = config
         self.armor_proposer = cv_armor_proposer(self.CFG, detect_color)
         self.digit_classifier = dl_digit_classifier(self.CFG, model_path)
diff --git a/config.py b/config.py
index ad4a3c4..7ae8f1b 100644
--- a/config.py
+++ b/config.py
@@ -21,4 +21,4 @@
 IMG_CENTER_X = IMG_WIDTH // 2
 IMG_CENTER_Y = IMG_HEIGHT // 2
 
-DEBUG_DISPLAY = False
+DEBUG_DISPLAY = True
diff --git a/vision.py b/vision.py
index 56b4426..8065ad7 100644
--- a/vision.py
+++ b/vision.py
@@ -43,7 +43,7 @@ def get_enemy_color(self):
 if __name__ == "__main__":
     model = cv_mix_dl_detector(config, DEFAULT_ENEMY_TEAM)
     # model = Yolo(config.MODEL_CFG_PATH, config.WEIGHT_PATH, config.META_PATH)
-    aimer = Aim()
+    aimer = Aim(config)
     communicator = serial_port
     pkt_seq = 0
 
@@ -65,6 +65,8 @@ def get_enemy_color(self):
                 byte_array = communicator.read(communicator.inWaiting())
                 my_color_buffer.receive(byte_array)
         
+        # TODO: add a global reset function if enemy functions change
+        # (e.g., clear the buffer in the armor tracker)
         enemy_team = my_color_buffer.get_enemy_color()
         model.change_color(enemy_team)
 
@@ -83,7 +85,7 @@ def get_enemy_color(self):
 
         if config.DEBUG_DISPLAY:
             viz_frame = frame.copy()
-            for name, conf, bbox in pred:
+            for _, _, bbox in pred:
                 lower_x = int(bbox[0] - bbox[2] / 2)
                 lower_y = int(bbox[1] - bbox[3] / 2)
                 upper_x = int(bbox[0] + bbox[2] / 2)
@@ -94,8 +96,26 @@ def get_enemy_color(self):
                 exit(0)
         
         # Tracking and filtering
+        # Pour all predictions into the aimer, which returns relative angles
         ret_dict = aimer.process_one(pred, enemy_team, frame)
 
+        if config.DEBUG_DISPLAY:
+            viz_frame = frame.copy()
+            for i in range(len(ret_dict['final_bbox_list'])):
+                bbox = ret_dict['final_bbox_list'][i]
+                unique_id = ret_dict['final_id_list'][i]
+                lower_x = int(bbox[0] - bbox[2] / 2)
+                lower_y = int(bbox[1] - bbox[3] / 2)
+                upper_x = int(bbox[0] + bbox[2] / 2)
+                upper_y = int(bbox[1] + bbox[3] / 2)
+                viz_frame = cv2.rectangle(viz_frame, (lower_x, lower_y), (upper_x, upper_y), (0, 255, 0), 2)
+                viz_frame = cv2.putText(viz_frame, str(unique_id), (lower_x, lower_y),
+                                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+            cv2.imshow('filtered_detected', viz_frame)
+            if cv2.waitKey(1) & 0xFF == ord('q'):
+                exit(0)
+
+        # TODO: put this into debug display
         show_frame = frame.copy()
 
         if ret_dict:

From 0a853c471f9f7e9e3d3999caf0583551f917e662 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Tue, 14 Feb 2023 03:31:53 -0600
Subject: [PATCH 22/25] slow down increments of armor IDs

---
 Aiming/Tracking/basic_tracker.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/Aiming/Tracking/basic_tracker.py b/Aiming/Tracking/basic_tracker.py
index f1543a3..967b1cb 100644
--- a/Aiming/Tracking/basic_tracker.py
+++ b/Aiming/Tracking/basic_tracker.py
@@ -52,7 +52,7 @@ class basic_tracker(object):
 
     It memorizes the state of last two predictions and do linear extrapolation
     '''
-    SE_THRESHOLD = 1800 # (30, 30) pixels away
+    SE_THRESHOLD = 3200 # (40, 40) pixels away
 
     def __init__(self, config):
         self.CFG = config
@@ -69,7 +69,7 @@ def process_one(self, pred_list, enemy_team, rgb_img):
             lower_y = int(c_y - h / 2)
             upper_y = int(c_y + h / 2)
             roi = rgb_img[lower_y:upper_y,lower_x:upper_x]
-            new_armors.append(tracked_armor(bbox, roi, self.frame_tick, self.id_gen.get_id()))
+            new_armors.append(tracked_armor(bbox, roi, self.frame_tick, -1))
 
         if len(self.active_armors) > 0:
             # Try to associate with current armors
@@ -88,6 +88,9 @@ def process_one(self, pred_list, enemy_team, rgb_img):
         
         new_armors = [i for i in new_armors if i is not None]
 
+        for n in new_armors:
+            n.armor_id = self.id_gen.get_id()
+
         # Maintain current buffer. If an armor has not been observed by 10 frames, it is dropped
         self.active_armors = [i for i in self.active_armors
                                 if self.frame_tick - i.observed_frame_tick[-1] < FRAME_BUFFER_SIZE]

From cdf2d06747ec457878b8aa4ab15b2905e3315675 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Tue, 14 Feb 2023 03:40:43 -0600
Subject: [PATCH 23/25] use main function in vision.py for debugging

---
 vision.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/vision.py b/vision.py
index 8065ad7..354c61e 100644
--- a/vision.py
+++ b/vision.py
@@ -40,7 +40,7 @@ def get_enemy_color(self):
         
         return self.default_color
 
-if __name__ == "__main__":
+def main():
     model = cv_mix_dl_detector(config, DEFAULT_ENEMY_TEAM)
     # model = Yolo(config.MODEL_CFG_PATH, config.WEIGHT_PATH, config.META_PATH)
     aimer = Aim(config)
@@ -139,3 +139,6 @@ def get_enemy_color(self):
             communicator.write(packet)
 
         pkt_seq += 1
+
+if __name__ == "__main__":
+    main()

From 7f20d7f345fafddc1c771d57684f299d1fb40646 Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Tue, 14 Feb 2023 03:51:43 -0600
Subject: [PATCH 24/25] update TODOs

---
 README.md | 8 ++++++++
 config.py | 2 +-
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 121f41e..7304b3f 100644
--- a/README.md
+++ b/README.md
@@ -14,6 +14,14 @@ them at the [UofI box](https://uofi.box.com/s/i6zahotr9id35hurjzy2bq3dcfz0085e).
 
 Please follow instruction from the RMCV101 repo [here](https://github.com/illini-robomaster/RM_CV_101/blob/master/INSTALL.md).
 
+## TODOs
+
+- Implement a more robust tracker (e.g., EKF / Kalman)
+- Implement depth estimators (either use D455 or estimate from monocular cameras)
+- Optimize code efficiency on Jetson
+- Implement communication protocol and test with embedded team
+- Implememt MDVS camera driver in Python
+
 ## File Structure
 
 ```
diff --git a/config.py b/config.py
index 7ae8f1b..ad4a3c4 100644
--- a/config.py
+++ b/config.py
@@ -21,4 +21,4 @@
 IMG_CENTER_X = IMG_WIDTH // 2
 IMG_CENTER_Y = IMG_HEIGHT // 2
 
-DEBUG_DISPLAY = True
+DEBUG_DISPLAY = False

From f4c2be12e7839ac98778cf4d6356eb246dcf130f Mon Sep 17 00:00:00 2001
From: RogerQi <rizhaoq2@illinois.edu>
Date: Tue, 14 Feb 2023 03:53:22 -0600
Subject: [PATCH 25/25] add CHANGELOG

---
 README.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/README.md b/README.md
index 7304b3f..c3b76c9 100644
--- a/README.md
+++ b/README.md
@@ -35,3 +35,6 @@ Please follow instruction from the RMCV101 repo [here](https://github.com/illini
 - config.py            --> global config (parameters and class instance)
 ```
 
+## CHANGLELOG
+
+2023-02-14 v0.0.1 release for 2023 season midterm. See notes [here](https://github.com/illini-robomaster/iRM_Vision_2023/pull/1).