From b159d7e05f44ea789f0869e3e35751af97f311bc Mon Sep 17 00:00:00 2001
From: Igor Maurell <igormaurell@gmail.com>
Date: Thu, 6 Jul 2023 18:37:36 -0300
Subject: [PATCH] fixing largest bbox bug in yolo tracker

---
 .../yolo_tracker_recognition.py               | 43 ++++++++-----------
 1 file changed, 19 insertions(+), 24 deletions(-)

diff --git a/butia_recognition/scripts/butia_recognition/yolo_tracker_recognition/yolo_tracker_recognition.py b/butia_recognition/scripts/butia_recognition/yolo_tracker_recognition/yolo_tracker_recognition.py
index 8ddc445..46849d0 100755
--- a/butia_recognition/scripts/butia_recognition/yolo_tracker_recognition/yolo_tracker_recognition.py
+++ b/butia_recognition/scripts/butia_recognition/yolo_tracker_recognition/yolo_tracker_recognition.py
@@ -36,6 +36,8 @@ def __init__(self,state = True):
             DeepOCSORT = boxmot.DeepOCSORT
         if self.tracking_on_init:
             self.startTracking(None)
+        
+        self.lastTrack = perf_counter()
 
         rospy.loginfo("Yolo Tracker Recognition Node started")
 
@@ -60,8 +62,8 @@ def serverStop(self, req):
     
     def startTracking(self, req):
         self.loadTrackerModel()
-        self.trackID = -2
-        self.lastTrack = -self.max_time
+        self.trackID = -1
+        self.lastTrack = perf_counter()
         self.tracking = True
         rospy.loginfo("Tracking started!!!")
         return EmptyResponse()
@@ -145,6 +147,10 @@ def callback(self, *args):
 
         tracked_box = None
         now = perf_counter()
+        is_aged = (now - self.lastTrack >= self.max_time)
+        is_id_found = False
+        previus_size = float("-inf")
+        new_id = -1
         # descriptions = []
         for i, box in enumerate(bboxs):
             description = Description2D()
@@ -165,7 +171,6 @@ def callback(self, *args):
             description.id = i
 
             box_label = ""
-            previus_size = float("-inf")
             if tracking:
                 description.global_id = ID
                 if description.label == "person":
@@ -173,43 +178,33 @@ def callback(self, *args):
                 
                 box_label = f"ID:{ID} "
                 size = description.bbox.size_x * description.bbox.size_y
-                # if ID == self.trackID or \
-                #     self.trackID == -1 or \
-                #     (perf_counter() - self.lastTrack >= self.max_time and \
-                #     (tracked_box == None or\
-                #     size > previus_size)):
-
-                #     self.trackID = ID
-                #     previus_size = size
-                #     tracked_box = description
+
                 if ID == self.trackID:
-                    self.trackID = ID
-                    previus_size = size
-                    tracked_box = description
-                    self.lastTrack = perf_counter()
-                elif perf_counter() - self.lastTrack >= self.max_time and (tracked_box == None or size > previus_size): 
-                    previus_size = size
+                    is_id_found = True
                     tracked_box = description
-                    self.trackID = ID
-                 
+
+                if (not is_id_found) and (is_aged or self.trackID == -1):
+                    if tracked_box is None or size > previus_size:
+                        previus_size = size
+                        tracked_box = description
+                        new_id = ID                    
 
             recognition.descriptions.append(description)
 
             cv.rectangle(debug_img,(int(X1),int(Y1)), (int(X2),int(Y2)),(0,0,255),thickness=2)
             cv.putText(debug_img,f"{box_label}{self.model.names[clss]}:{score:.2f}", (int(X1), int(Y1)), cv.FONT_HERSHEY_SIMPLEX,0.75,(0,0,255),thickness=2)
-        if tracked_box != None:
-            self.lastTrack = perf_counter()
-
         
         track_recognition = Recognitions2D()
         track_recognition.header = HEADER
         tracked_description : Description2D = deepcopy(tracked_box)
-        if tracked_box != None:
+        if tracked_box is not None:
             track_recognition.header = recognition.header
             track_recognition.image_rgb = recognition.image_rgb
             track_recognition.image_depth = recognition.image_depth
             track_recognition.camera_info = recognition.camera_info
             tracked_description.type = Description2D.DETECTION
+            if not is_id_found:
+                self.trackID = new_id
             # recognition.descriptions.append(tracked_box)
             self.lastTrack = now
             cv.rectangle(debug_img,(int(tracked_box.bbox.center.x-tracked_box.bbox.size_x/2),\