From 95ff455d5b0ca0b93e7e3fcbd101c59af86a94ea Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 12:07:27 +0100 Subject: [PATCH 01/28] add init parameter --- boxmot/trackers/ocsort/ocsort.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 0131463a32..a1d8f48fdf 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -208,7 +208,8 @@ def __init__( inertia: float = 0.2, use_byte: bool = False, Q_xy_scaling: float = 0.01, - Q_s_scaling: float = 0.0001 + Q_s_scaling: float = 0.0001, + is_obb: bool = False ): super().__init__(max_age=max_age, per_class=per_class, asso_func=asso_func) """ From 97af95e36b0bdad24368c7075051a9652e574b26 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 12:32:11 +0100 Subject: [PATCH 02/28] add `centroid_batch_obb` method --- boxmot/utils/iou.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/boxmot/utils/iou.py b/boxmot/utils/iou.py index f1b53dfc31..ed24188602 100644 --- a/boxmot/utils/iou.py +++ b/boxmot/utils/iou.py @@ -144,6 +144,19 @@ def centroid_batch(self, bboxes1, bboxes2) -> np.ndarray: return 1 - normalized_distances + def centroid_batch_obb(self, bboxes1, bboxes2) -> np.ndarray: + centroids1 = np.stack((bboxes1[..., 0], bboxes1[..., 1]),axis=-1) + centroids2 = np.stack((bboxes2[..., 0], bboxes2[..., 1]),axis=-1) + + centroids1 = np.expand_dims(centroids1, 1) + centroids2 = np.expand_dims(centroids2, 0) + + distances = np.sqrt(np.sum((centroids1 - centroids2) ** 2, axis=-1)) + norm_factor = np.sqrt(self.w ** 2 + self.h ** 2) + normalized_distances = distances / norm_factor + + return 1 - normalized_distances + @staticmethod def ciou_batch(bboxes1, bboxes2) -> np.ndarray: From 5e5908f397602a0b424631bb60e72942f0b6d862 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 12:33:11 +0100 Subject: [PATCH 03/28] add "centroid_obb" to `ASSO_FUNC` --- boxmot/utils/iou.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/boxmot/utils/iou.py b/boxmot/utils/iou.py index ed24188602..fdcb32188f 100644 --- a/boxmot/utils/iou.py +++ b/boxmot/utils/iou.py @@ -34,8 +34,7 @@ def iou_batch(bboxes1, bboxes2) -> np.ndarray: wh ) return o - - + @staticmethod def hmiou_batch(bboxes1, bboxes2): """ @@ -296,7 +295,8 @@ def _get_asso_func(self, asso_mode): "giou": AssociationFunction.giou_batch, "ciou": AssociationFunction.ciou_batch, "diou": AssociationFunction.diou_batch, - "centroid": self.centroid_batch # only not being staticmethod + "centroid": self.centroid_batch, # only not being staticmethod + "centroid_obb": self.centroid_batch_obb } if self.asso_mode not in ASSO_FUNCS: From 7992a536c21a050fb6b11a8370f517e36476f037 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 12:34:22 +0100 Subject: [PATCH 04/28] add `is_obb` BasTracker init parameter --- boxmot/trackers/basetracker.py | 3 ++- boxmot/trackers/ocsort/ocsort.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index c56ac0f669..bebb492e92 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -17,7 +17,8 @@ def __init__( max_obs: int = 50, nr_classes: int = 80, per_class: bool = False, - asso_func: str = 'iou' + asso_func: str = 'iou', + is_obb: bool = False ): """ Initialize the BaseTracker object with detection threshold, maximum age, minimum hits, diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index a1d8f48fdf..09bca97bb2 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -211,7 +211,7 @@ def __init__( Q_s_scaling: float = 0.0001, is_obb: bool = False ): - super().__init__(max_age=max_age, per_class=per_class, asso_func=asso_func) + super().__init__(max_age=max_age, per_class=per_class, asso_func=asso_func, is_obb=is_obb) """ Sets key parameters for SORT """ From 00cfe9c0c84f90d59042a27dbb6150c22c3bd961 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 12:36:07 +0100 Subject: [PATCH 05/28] `self.asso_func_name` changes depends on `is_obb`, just add "_obb" sub string to match AssociationFunction.ASSO_FUNCS --- boxmot/trackers/basetracker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index bebb492e92..21a2e44abe 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -42,7 +42,7 @@ def __init__( self.nr_classes = nr_classes self.iou_threshold = iou_threshold self.last_emb_size = None - self.asso_func_name = asso_func + self.asso_func_name = asso_func+"_obb" if is_obb else asso_func self.frame_count = 0 self.active_tracks = [] # This might be handled differently in derived classes From ce6c2661c743471cd97ab8f79a3e9a2125e9e042 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 12:57:41 +0100 Subject: [PATCH 06/28] adpat check_input based on self.is_obb --- boxmot/trackers/basetracker.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index 21a2e44abe..f36a72c58b 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -179,9 +179,16 @@ def check_inputs(self, dets, img): assert ( len(dets.shape) == 2 ), "Unsupported 'dets' dimensions, valid number of dimensions is two" - assert ( - dets.shape[1] == 6 - ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6" + if self.is_obb : + + assert ( + dets.shape[1] == 6 + ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6 (x1,y1,x2,y2,conf,cls)" + else : + assert ( + dets.shape[1] == 7 + ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6 (cx,cy,w,h,angle,conf,cls)" + def id_to_color(self, id: int, saturation: float = 0.75, value: float = 0.95) -> tuple: """ From fb0f76d04f6e25f602e3c4ca1f97f1664acc6cd9 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 13:02:15 +0100 Subject: [PATCH 07/28] add is_obb to Basetracker attribute --- boxmot/trackers/basetracker.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index f36a72c58b..ae97a62252 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -43,7 +43,8 @@ def __init__( self.iou_threshold = iou_threshold self.last_emb_size = None self.asso_func_name = asso_func+"_obb" if is_obb else asso_func - + self.is_obb = is_obb + self.frame_count = 0 self.active_tracks = [] # This might be handled differently in derived classes self.per_class_active_tracks = None From 747c2485a4df9938246d39f2cbdec1371840c543 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 14:12:08 +0100 Subject: [PATCH 08/28] reindexing dets[i+self.is_obb], especilly for KFTracker.update() a good way should be to decorate the method to handle dets[idx] args --- boxmot/trackers/ocsort/ocsort.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 09bca97bb2..7dcb1c5e1f 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -246,7 +246,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> h, w = img.shape[0:2] dets = np.hstack([dets, np.arange(len(dets)).reshape(-1, 1)]) - confs = dets[:, 4] + confs = dets[:, 4+self.is_obb] inds_low = confs > 0.1 inds_high = confs < self.det_thresh @@ -258,12 +258,12 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> dets = dets[remain_inds] # get predicted locations from existing trackers. - trks = np.zeros((len(self.active_tracks), 5)) + trks = np.zeros((len(self.active_tracks), 5+self.is_obb)) to_del = [] ret = [] for t, trk in enumerate(trks): pos = self.active_tracks[t].predict()[0] - trk[:] = [pos[0], pos[1], pos[2], pos[3], 0] + trk[:] = [pos[i] for i in range(4+self.is_obb)] + [0] if np.any(np.isnan(pos)): to_del.append(t) trks = np.ma.compress_rows(np.ma.masked_invalid(trks)) @@ -288,10 +288,10 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> First round of association """ matched, unmatched_dets, unmatched_trks = associate( - dets[:, 0:5], trks, self.asso_func, self.asso_threshold, velocities, k_observations, self.inertia, w, h + dets[:, 0:5+self.is_obb], trks, self.asso_func, self.asso_threshold, velocities, k_observations, self.inertia, w, h ) for m in matched: - self.active_tracks[m[1]].update(dets[m[0], :5], dets[m[0], 5], dets[m[0], 6]) + self.active_tracks[m[1]].update(dets[m[0], :5+self.is_obb], dets[m[0], 5+self.is_obb], dets[m[0], 6+self.is_obb]) """ Second round of associaton by OCR @@ -316,7 +316,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> if iou_left[m[0], m[1]] < self.asso_threshold: continue self.active_tracks[trk_ind].update( - dets_second[det_ind, :5], dets_second[det_ind, 5], dets_second[det_ind, 6] + dets_second[det_ind, :5+self.is_obb], dets_second[det_ind, 5+self.is_obb], dets_second[det_ind, 6+self.is_obb] ) to_remove_trk_indices.append(trk_ind) unmatched_trks = np.setdiff1d( @@ -341,7 +341,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> det_ind, trk_ind = unmatched_dets[m[0]], unmatched_trks[m[1]] if iou_left[m[0], m[1]] < self.asso_threshold: continue - self.active_tracks[trk_ind].update(dets[det_ind, :5], dets[det_ind, 5], dets[det_ind, 6]) + self.active_tracks[trk_ind].update(dets[det_ind, :5+self.is_obb], dets[det_ind, 5+self.is_obb], dets[det_ind, 6+self.is_obb]) to_remove_det_indices.append(det_ind) to_remove_trk_indices.append(trk_ind) unmatched_dets = np.setdiff1d( @@ -356,7 +356,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> # create and initialise new trackers for unmatched detections for i in unmatched_dets: - trk = KalmanBoxTracker(dets[i, :5], dets[i, 5], dets[i, 6], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_s_scaling=self.Q_s_scaling, max_obs=self.max_obs) + trk = KalmanBoxTracker(dets[i, :5+self.is_obb], dets[i, 5+self.is_obb], dets[i, 6+self.is_obb], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_s_scaling=self.Q_s_scaling, max_obs=self.max_obs) self.active_tracks.append(trk) i = len(self.active_tracks) for trk in reversed(self.active_tracks): @@ -367,7 +367,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> this is optional to use the recent observation or the kalman filter prediction, we didn't notice significant difference here """ - d = trk.last_observation[:4] + d = trk.last_observation[:4+self.is_obb] if (trk.time_since_update < 1) and ( trk.hit_streak >= self.min_hits or self.frame_count <= self.min_hits ): From 7522b9d4f8e26fa1e68f2d960d9b04552bd6b177 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 14:39:19 +0100 Subject: [PATCH 09/28] Copy xysr.py and adapt unfreeze method --- boxmot/motion/kalman_filters/xywha.py | 468 ++++++++++++++++++++++++++ 1 file changed, 468 insertions(+) create mode 100644 boxmot/motion/kalman_filters/xywha.py diff --git a/boxmot/motion/kalman_filters/xywha.py b/boxmot/motion/kalman_filters/xywha.py new file mode 100644 index 0000000000..73d1b04d5f --- /dev/null +++ b/boxmot/motion/kalman_filters/xywha.py @@ -0,0 +1,468 @@ +""" +This module implements the linear Kalman filter in both an object +oriented and procedural form. The KalmanFilter class implements +the filter by storing the various matrices in instance variables, +minimizing the amount of bookkeeping you have to do. +All Kalman filters operate with a predict->update cycle. The +predict step, implemented with the method or function predict(), +uses the state transition matrix F to predict the state in the next +time period (epoch). The state is stored as a gaussian (x, P), where +x is the state (column) vector, and P is its covariance. Covariance +matrix Q specifies the process covariance. In Bayesian terms, this +prediction is called the *prior*, which you can think of colloquially +as the estimate prior to incorporating the measurement. +The update step, implemented with the method or function `update()`, +incorporates the measurement z with covariance R, into the state +estimate (x, P). The class stores the system uncertainty in S, +the innovation (residual between prediction and measurement in +measurement space) in y, and the Kalman gain in k. The procedural +form returns these variables to you. In Bayesian terms this computes +the *posterior* - the estimate after the information from the +measurement is incorporated. +Whether you use the OO form or procedural form is up to you. If +matrices such as H, R, and F are changing each epoch, you'll probably +opt to use the procedural form. If they are unchanging, the OO +form is perhaps easier to use since you won't need to keep track +of these matrices. This is especially useful if you are implementing +banks of filters or comparing various KF designs for performance; +a trivial coding bug could lead to using the wrong sets of matrices. +This module also offers an implementation of the RTS smoother, and +other helper functions, such as log likelihood computations. +The Saver class allows you to easily save the state of the +KalmanFilter class after every update. +""" + +from __future__ import absolute_import, division + +from copy import deepcopy +from math import log, exp, sqrt +import sys +import numpy as np +from numpy import dot, zeros, eye, isscalar, shape +import numpy.linalg as linalg +from filterpy.stats import logpdf +from filterpy.common import pretty_str, reshape_z +from collections import deque + + +class KalmanFilterXYWHA(object): + """ Implements a Kalman filter. You are responsible for setting the + various state variables to reasonable values; the defaults will + not give you a functional filter. + """ + + def __init__(self, dim_x, dim_z, dim_u=0, max_obs=50): + if dim_x < 1: + raise ValueError('dim_x must be 1 or greater') + if dim_z < 1: + raise ValueError('dim_z must be 1 or greater') + if dim_u < 0: + raise ValueError('dim_u must be 0 or greater') + + self.dim_x = dim_x + self.dim_z = dim_z + self.dim_u = dim_u + + self.x = zeros((dim_x, 1)) # state + self.P = eye(dim_x) # uncertainty covariance + self.Q = eye(dim_x) # process uncertainty + self.B = None # control transition matrix + self.F = eye(dim_x) # state transition matrix + self.H = zeros((dim_z, dim_x)) # measurement function + self.R = eye(dim_z) # measurement uncertainty + self._alpha_sq = 1. # fading memory control + self.M = np.zeros((dim_x, dim_z)) # process-measurement cross correlation + self.z = np.array([[None]*self.dim_z]).T + + # gain and residual are computed during the innovation step. We + # save them so that in case you want to inspect them for various + # purposes + self.K = np.zeros((dim_x, dim_z)) # kalman gain + self.y = zeros((dim_z, 1)) + self.S = np.zeros((dim_z, dim_z)) # system uncertainty + self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty + + # identity matrix. Do not alter this. + self._I = np.eye(dim_x) + + # these will always be a copy of x,P after predict() is called + self.x_prior = self.x.copy() + self.P_prior = self.P.copy() + + # these will always be a copy of x,P after update() is called + self.x_post = self.x.copy() + self.P_post = self.P.copy() + + # Only computed only if requested via property + self._log_likelihood = log(sys.float_info.min) + self._likelihood = sys.float_info.min + self._mahalanobis = None + + # keep all observations + self.max_obs = max_obs + self.history_obs = deque([], maxlen=self.max_obs) + + self.inv = np.linalg.inv + + self.attr_saved = None + self.observed = False + self.last_measurement = None + + + def apply_affine_correction(self, m, t): + #TODO ADAPT FOR OBB + """ + Apply to both last state and last observation for OOS smoothing. + + Messy due to internal logic for kalman filter being messy. + """ + + scale = np.linalg.norm(m[:, 0]) + self.x[:2] = m @ self.x[:2] + t + self.x[4:6] = m @ self.x[4:6] + + self.P[:2, :2] = m @ self.P[:2, :2] @ m.T + self.P[4:6, 4:6] = m @ self.P[4:6, 4:6] @ m.T + + # If frozen, also need to update the frozen state for OOS + if not self.observed and self.attr_saved is not None: + self.attr_saved["x"][:2] = m @ self.attr_saved["x"][:2] + t + self.attr_saved["x"][4:6] = m @ self.attr_saved["x"][4:6] + + self.attr_saved["P"][:2, :2] = m @ self.attr_saved["P"][:2, :2] @ m.T + self.attr_saved["P"][4:6, 4:6] = m @ self.attr_saved["P"][4:6, 4:6] @ m.T + + self.attr_saved["last_measurement"][:2] = m @ self.attr_saved["last_measurement"][:2] + t + + + def predict(self, u=None, B=None, F=None, Q=None): + """ + Predict next state (prior) using the Kalman filter state propagation + equations. + Parameters + ---------- + u : np.array, default 0 + Optional control vector. + B : np.array(dim_x, dim_u), or None + Optional control transition matrix; a value of None + will cause the filter to use `self.B`. + F : np.array(dim_x, dim_x), or None + Optional state transition matrix; a value of None + will cause the filter to use `self.F`. + Q : np.array(dim_x, dim_x), scalar, or None + Optional process noise matrix; a value of None will cause the + filter to use `self.Q`. + """ + if B is None: + B = self.B + if F is None: + F = self.F + if Q is None: + Q = self.Q + elif isscalar(Q): + Q = eye(self.dim_x) * Q + + # x = Fx + Bu + if B is not None and u is not None: + self.x = dot(F, self.x) + dot(B, u) + else: + self.x = dot(F, self.x) + + # P = FPF' + Q + self.P = self._alpha_sq * dot(dot(F, self.P), F.T) + Q + + # save prior + self.x_prior = self.x.copy() + self.P_prior = self.P.copy() + + def freeze(self): + """ + Save the parameters before non-observation forward + """ + self.attr_saved = deepcopy(self.__dict__) + + def unfreeze(self): + if self.attr_saved is not None: + new_history = deepcopy(list(self.history_obs)) + self.__dict__ = self.attr_saved + self.history_obs = deque(list(self.history_obs)[:-1], maxlen=self.max_obs) + occur = [int(d is None) for d in new_history] + indices = np.where(np.array(occur) == 0)[0] + index1, index2 = indices[-2], indices[-1] + box1, box2 = new_history[index1], new_history[index2] + x1, y1, w1, h1, a1 = box1 + x2, y2, w2, h2, a2 = box2 + time_gap = index2 - index1 + dx, dy = (x2 - x1) / time_gap, (y2 - y1) / time_gap + dw, dh = (w2 - w1) / time_gap, (h2 - h1) / time_gap + da = (a2 - a1) / time_gap + for i in range(index2 - index1): + x, y = x1 + (i + 1) * dx, y1 + (i + 1) * dy + w, h = w1 + (i + 1) * dw, h1 + (i + 1) * dh + a = a1 + (i + 1) * da + new_box = np.array([x, y, w, h, a]).reshape((5, 1)) + self.update(new_box) + if not i == (index2 - index1 - 1): + self.predict() + self.history_obs.pop() + self.history_obs.pop() + + def update(self, z, R=None, H=None): + """ + Add a new measurement (z) to the Kalman filter. If z is None, nothing is changed. + Parameters + ---------- + z : np.array + Measurement for this update. z can be a scalar if dim_z is 1, + otherwise it must be a column vector. + R : np.array, scalar, or None + Measurement noise. If None, the filter's self.R value is used. + H : np.array, or None + Measurement function. If None, the filter's self.H value is used. + """ + + # set to None to force recompute + self._log_likelihood = None + self._likelihood = None + self._mahalanobis = None + + # append the observation + self.history_obs.append(z) + + if z is None: + if self.observed: + """ + Got no observation so freeze the current parameters for future + potential online smoothing. + """ + self.last_measurement = self.history_obs[-2] + self.freeze() + self.observed = False + self.z = np.array([[None] * self.dim_z]).T + self.x_post = self.x.copy() + self.P_post = self.P.copy() + self.y = zeros((self.dim_z, 1)) + return + + # self.observed = True + if not self.observed: + """ + Get observation, use online smoothing to re-update parameters + """ + self.unfreeze() + self.observed = True + + if R is None: + R = self.R + elif isscalar(R): + R = eye(self.dim_z) * R + if H is None: + z = reshape_z(z, self.dim_z, self.x.ndim) + H = self.H + + # y = z - Hx + # error (residual) between measurement and prediction + self.y = z - dot(H, self.x) + + # common subexpression for speed + PHT = dot(self.P, H.T) + + # S = HPH' + R + self.S = dot(H, PHT) + R + self.SI = self.inv(self.S) + + # K = PH'inv(S) + self.K = PHT.dot(self.SI) + + # x = x + Ky + self.x = self.x + dot(self.K, self.y) + + # P = (I-KH)P(I-KH)' + KRK' + I_KH = self._I - dot(self.K, H) + self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T) + + # save measurement and posterior state + self.z = deepcopy(z) + self.x_post = self.x.copy() + self.P_post = self.P.copy() + + # save history of observations + self.history_obs.append(z) + + def update_steadystate(self, z, H=None): + """ Update Kalman filter using the Kalman gain and state covariance + matrix as computed for the steady state. Only x is updated, and the + new value is stored in self.x. P is left unchanged. Must be called + after a prior call to compute_steady_state(). + """ + if z is None: + self.history_obs.append(z) + return + + if H is None: + H = self.H + + H = np.asarray(H) + # error (residual) between measurement and prediction + self.y = z - dot(H, self.x) + + # x = x + Ky + self.x = self.x + dot(self.K_steady_state, self.y) + + # save measurement and posterior state + self.z = deepcopy(z) + self.x_post = self.x.copy() + + # save history of observations + self.history_obs.append(z) + + def log_likelihood(self, z=None): + """ log-likelihood of the measurement z. Computed from the + system uncertainty S. + """ + + if z is None: + z = self.z + return logpdf(z, dot(self.H, self.x), self.S) + + def likelihood(self, z=None): + """ likelihood of the measurement z. Computed from the + system uncertainty S. + """ + + if z is None: + z = self.z + return exp(self.log_likelihood(z)) + + @property + def log_likelihood(self): + """ log-likelihood of the last measurement. + """ + + return self._log_likelihood + + @property + def likelihood(self): + """ likelihood of the last measurement. + """ + + return self._likelihood + + +def batch_filter(x, P, zs, Fs, Qs, Hs, Rs, Bs=None, us=None, update_first=False, saver=None): + """ + Batch processes a sequences of measurements. + Parameters + ---------- + zs : list-like + list of measurements at each time step. Missing measurements must be + represented by None. + Fs : list-like + list of values to use for the state transition matrix matrix. + Qs : list-like + list of values to use for the process error + covariance. + Hs : list-like + list of values to use for the measurement matrix. + Rs : list-like + list of values to use for the measurement error + covariance. + Bs : list-like, optional + list of values to use for the control transition matrix; + a value of None in any position will cause the filter + to use `self.B` for that time step. + us : list-like, optional + list of values to use for the control input vector; + a value of None in any position will cause the filter to use + 0 for that time step. + update_first : bool, optional + controls whether the order of operations is update followed by + predict, or predict followed by update. Default is predict->update. + saver : filterpy.common.Saver, optional + filterpy.common.Saver object. If provided, saver.save() will be + called after every epoch + Returns + ------- + means : np.array((n,dim_x,1)) + array of the state for each time step after the update. Each entry + is an np.array. In other words `means[k,:]` is the state at step + `k`. + covariance : np.array((n,dim_x,dim_x)) + array of the covariances for each time step after the update. + In other words `covariance[k,:,:]` is the covariance at step `k`. + means_predictions : np.array((n,dim_x,1)) + array of the state for each time step after the predictions. Each + entry is an np.array. In other words `means[k,:]` is the state at + step `k`. + covariance_predictions : np.array((n,dim_x,dim_x)) + array of the covariances for each time step after the prediction. + In other words `covariance[k,:,:]` is the covariance at step `k`. + Examples + -------- + .. code-block:: Python + zs = [t + random.randn()*4 for t in range (40)] + Fs = [kf.F for t in range (40)] + Hs = [kf.H for t in range (40)] + (mu, cov, _, _) = kf.batch_filter(zs, Rs=R_list, Fs=Fs, Hs=Hs, Qs=None, + Bs=None, us=None, update_first=False) + (xs, Ps, Ks, Pps) = kf.rts_smoother(mu, cov, Fs=Fs, Qs=None) + """ + + n = np.size(zs, 0) + dim_x = x.shape[0] + + # mean estimates from Kalman Filter + if x.ndim == 1: + means = zeros((n, dim_x)) + means_p = zeros((n, dim_x)) + else: + means = zeros((n, dim_x, 1)) + means_p = zeros((n, dim_x, 1)) + + # state covariances from Kalman Filter + covariances = zeros((n, dim_x, dim_x)) + covariances_p = zeros((n, dim_x, dim_x)) + + if us is None: + us = [0.0] * n + Bs = [0.0] * n + + if update_first: + for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): + + x, P = update(x, P, z, R=R, H=H) + means[i, :] = x + covariances[i, :, :] = P + + x, P = predict(x, P, u=u, B=B, F=F, Q=Q) + means_p[i, :] = x + covariances_p[i, :, :] = P + if saver is not None: + saver.save() + else: + for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): + + x, P = predict(x, P, u=u, B=B, F=F, Q=Q) + means_p[i, :] = x + covariances_p[i, :, :] = P + + x, P = update(x, P, z, R=R, H=H) + means[i, :] = x + covariances[i, :, :] = P + if saver is not None: + saver.save() + + return (means, covariances, means_p, covariances_p) + + def batch_filter(self, zs, Rs=None): + """ + Batch process a sequence of measurements. This method is suitable + for cases where the measurement noise varies with each measurement. + """ + means, covariances = [], [] + for z, R in zip(zs, Rs): + self.predict() + self.update(z, R=R) + means.append(self.x.copy()) + covariances.append(self.P.copy()) + return np.array(means), np.array(covariances) \ No newline at end of file From 820017da8fc646e820f779ba5cc5286840382211 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 14:56:53 +0100 Subject: [PATCH 10/28] copy KalmanBoxTracker into KalmanBoxTrackerOBB --- boxmot/trackers/ocsort/ocsort.py | 130 +++++++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 7dcb1c5e1f..0726f33a1d 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -179,6 +179,136 @@ def get_state(self): return convert_x_to_bbox(self.kf.x) +class KalmanBoxTrackerOBB(object): + """ + This class represents the internal state of individual tracked objects observed as oriented bbox. + """ + + count = 0 + + def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_s_scaling = 0.0001): + """ + Initialises a tracker using initial bounding box. + + """ + # define constant velocity model + self.det_ind = det_ind + + self.Q_xy_scaling = Q_xy_scaling + self.Q_s_scaling = Q_s_scaling + + self.kf = KalmanFilterXYSR(dim_x=7, dim_z=4, max_obs=max_obs) + self.kf.F = np.array( + [ + [1, 0, 0, 0, 1, 0, 0], + [0, 1, 0, 0, 0, 1, 0], + [0, 0, 1, 0, 0, 0, 1], + [0, 0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 0, 1], + ] + ) + self.kf.H = np.array( + [ + [1, 0, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 1, 0, 0, 0], + ] + ) + + self.kf.R[2:, 2:] *= 10.0 + self.kf.P[ + 4:, 4: + ] *= 1000.0 # give high uncertainty to the unobservable initial velocities + self.kf.P *= 10.0 + + self.kf.Q[4:6, 4:6] *= self.Q_xy_scaling + self.kf.Q[-1, -1] *= self.Q_s_scaling + + self.kf.x[:4] = xyxy2xysr(bbox) + self.time_since_update = 0 + self.id = KalmanBoxTracker.count + KalmanBoxTracker.count += 1 + self.max_obs = max_obs + self.history = deque([], maxlen=self.max_obs) + self.hits = 0 + self.hit_streak = 0 + self.age = 0 + self.conf = bbox[-1] + self.cls = cls + """ + NOTE: [-1,-1,-1,-1,-1] is a compromising placeholder for non-observation status, the same for the return of + function k_previous_obs. It is ugly and I do not like it. But to support generate observation array in a + fast and unified way, which you would see below k_observations = np.array([k_previous_obs(...]]), + let's bear it for now. + """ + self.last_observation = np.array([-1, -1, -1, -1, -1]) # placeholder + self.observations = dict() + self.history_observations = deque([], maxlen=self.max_obs) + self.velocity = None + self.delta_t = delta_t + + def update(self, bbox, cls, det_ind): + """ + Updates the state vector with observed bbox. + """ + self.det_ind = det_ind + if bbox is not None: + self.conf = bbox[-1] + self.cls = cls + if self.last_observation.sum() >= 0: # no previous observation + previous_box = None + for i in range(self.delta_t): + dt = self.delta_t - i + if self.age - dt in self.observations: + previous_box = self.observations[self.age - dt] + break + if previous_box is None: + previous_box = self.last_observation + """ + Estimate the track speed direction with observations \Delta t steps away + """ + self.velocity = speed_direction(previous_box, bbox) + + """ + Insert new observations. This is a ugly way to maintain both self.observations + and self.history_observations. Bear it for the moment. + """ + self.last_observation = bbox + self.observations[self.age] = bbox + self.history_observations.append(bbox) + + self.time_since_update = 0 + self.hits += 1 + self.hit_streak += 1 + self.kf.update(xyxy2xysr(bbox)) + else: + self.kf.update(bbox) + + def predict(self): + """ + Advances the state vector and returns the predicted bounding box estimate. + """ + if (self.kf.x[6] + self.kf.x[2]) <= 0: + self.kf.x[6] *= 0.0 + + self.kf.predict() + self.age += 1 + if self.time_since_update > 0: + self.hit_streak = 0 + self.time_since_update += 1 + self.history.append(convert_x_to_bbox(self.kf.x)) + return self.history[-1] + + def get_state(self): + """ + Returns the current bounding box estimate. + """ + return convert_x_to_bbox(self.kf.x) + + class OcSort(BaseTracker): """ OCSort Tracker: A tracking algorithm that utilizes motion-based tracking. From 82f38bab7e997a9849b43c3812d3e6f638e2d5f5 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 15:08:34 +0100 Subject: [PATCH 11/28] rename xywha_kf.py --- boxmot/motion/kalman_filters/{xywha.py => xywha_kf.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename boxmot/motion/kalman_filters/{xywha.py => xywha_kf.py} (100%) diff --git a/boxmot/motion/kalman_filters/xywha.py b/boxmot/motion/kalman_filters/xywha_kf.py similarity index 100% rename from boxmot/motion/kalman_filters/xywha.py rename to boxmot/motion/kalman_filters/xywha_kf.py From 9adc4a5326089abdfed29cb8e834e80b913b2f58 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 15:09:18 +0100 Subject: [PATCH 12/28] adapt KalmanBoxTrackerOBB.__init__() --- boxmot/trackers/ocsort/ocsort.py | 60 +++++++++++++++++++------------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 0726f33a1d..18712eb321 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -8,6 +8,7 @@ from boxmot.motion.kalman_filters.xysr_kf import KalmanFilterXYSR +from boxmot.motion.kalman_filters.xywha_kf import KalmanFilterXYWHA from boxmot.utils.association import associate, linear_assignment from boxmot.trackers.basetracker import BaseTracker from boxmot.utils.ops import xyxy2xysr @@ -48,6 +49,13 @@ def speed_direction(bbox1, bbox2): norm = np.sqrt((cy2 - cy1) ** 2 + (cx2 - cx1) ** 2) + 1e-6 return speed / norm +def speed_direction_obb(bbox1, bbox2): + cx1, cy1 = bbox1[0], bbox1[1] + cx2, cy2 = bbox2[0], bbox2[1] + speed = np.array([cy2 - cy1, cx2 - cx1]) + norm = np.sqrt((cy2 - cy1) ** 2 + (cx2 - cx1) ** 2) + 1e-6 + return speed / norm + class KalmanBoxTracker(object): """ @@ -186,7 +194,7 @@ class KalmanBoxTrackerOBB(object): count = 0 - def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_s_scaling = 0.0001): + def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_a_scaling = 0.01): """ Initialises a tracker using initial bounding box. @@ -195,42 +203,46 @@ def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.0 self.det_ind = det_ind self.Q_xy_scaling = Q_xy_scaling - self.Q_s_scaling = Q_s_scaling + self.Q_a_scaling = Q_a_scaling - self.kf = KalmanFilterXYSR(dim_x=7, dim_z=4, max_obs=max_obs) + self.kf = KalmanFilterXYSR(dim_x=10, dim_z=5, max_obs=max_obs) self.kf.F = np.array( [ - [1, 0, 0, 0, 1, 0, 0], - [0, 1, 0, 0, 0, 1, 0], - [0, 0, 1, 0, 0, 0, 1], - [0, 0, 0, 1, 0, 0, 0], - [0, 0, 0, 0, 1, 0, 0], - [0, 0, 0, 0, 0, 1, 0], - [0, 0, 0, 0, 0, 0, 1], - ] - ) + [1, 0, 0, 0, 0, 1, 0, 0, 0, 0], # cx = cx + vx + [0, 1, 0, 0, 0, 0, 1, 0, 0, 0], # cy = cy + vy + [0, 0, 1, 0, 0, 0, 0, 1, 0, 0], # w = w + vw + [0, 0, 0, 1, 0, 0, 0, 0, 1, 0], # h = h + vh + [0, 0, 0, 0, 1, 0, 0, 0, 0, 1], # a = a + va + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 1] + ] + ) self.kf.H = np.array( [ - [1, 0, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0, 0], - [0, 0, 0, 1, 0, 0, 0], - ] + [1, 0, 0, 0, 0, 0, 0, 0, 0 ,0], # cx + [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], # cy + [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], # w + [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], # h + [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], # angle + ] ) self.kf.R[2:, 2:] *= 10.0 self.kf.P[ - 4:, 4: + 5:, 5: ] *= 1000.0 # give high uncertainty to the unobservable initial velocities self.kf.P *= 10.0 - self.kf.Q[4:6, 4:6] *= self.Q_xy_scaling - self.kf.Q[-1, -1] *= self.Q_s_scaling + self.kf.Q[5:7, 5:7] *= self.Q_xy_scaling + self.kf.Q[-1, -1] *= self.Q_a_scaling - self.kf.x[:4] = xyxy2xysr(bbox) + self.kf.x[:5] = bbox self.time_since_update = 0 - self.id = KalmanBoxTracker.count - KalmanBoxTracker.count += 1 + self.id = KalmanBoxTrackerOBB.count + KalmanBoxTrackerOBB.count += 1 self.max_obs = max_obs self.history = deque([], maxlen=self.max_obs) self.hits = 0 @@ -244,7 +256,7 @@ def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.0 fast and unified way, which you would see below k_observations = np.array([k_previous_obs(...]]), let's bear it for now. """ - self.last_observation = np.array([-1, -1, -1, -1, -1]) # placeholder + self.last_observation = np.array([-1, -1, -1, -1, -1, -1]) #WARNING : -1 is a valid angle value self.observations = dict() self.history_observations = deque([], maxlen=self.max_obs) self.velocity = None From 02bacf94d9d8b23bd115a1883d0c1d3578142c89 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 15:10:59 +0100 Subject: [PATCH 13/28] speed_velocity modif and convert format --- boxmot/trackers/ocsort/ocsort.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 18712eb321..c75fb6c753 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -282,7 +282,7 @@ def update(self, bbox, cls, det_ind): """ Estimate the track speed direction with observations \Delta t steps away """ - self.velocity = speed_direction(previous_box, bbox) + self.velocity = speed_direction_obb(previous_box, bbox) """ Insert new observations. This is a ugly way to maintain both self.observations @@ -295,7 +295,7 @@ def update(self, bbox, cls, det_ind): self.time_since_update = 0 self.hits += 1 self.hit_streak += 1 - self.kf.update(xyxy2xysr(bbox)) + self.kf.update(bbox) else: self.kf.update(bbox) From c5d39ab3222bbc2084ea09de6081bd274d70d8f7 Mon Sep 17 00:00:00 2001 From: LilBabines Date: Tue, 24 Dec 2024 15:14:27 +0100 Subject: [PATCH 14/28] adapt predict and get_state --- boxmot/trackers/ocsort/ocsort.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index c75fb6c753..55a47810fd 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -303,22 +303,23 @@ def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ - if (self.kf.x[6] + self.kf.x[2]) <= 0: - self.kf.x[6] *= 0.0 - + if (self.kf.x[7] + self.kf.x[2]) <= 0: # Negative width + self.kf.x[7] *= 0.0 + if (self.kf.x[8] + self.kf.x[3]) <= 0: # Negative Height + self.kf.x[8] *= 0.0 self.kf.predict() self.age += 1 if self.time_since_update > 0: self.hit_streak = 0 self.time_since_update += 1 - self.history.append(convert_x_to_bbox(self.kf.x)) + self.history.append(self.kf.x) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ - return convert_x_to_bbox(self.kf.x) + return self.kf.x class OcSort(BaseTracker): From 1d39ac142b6c075e4f915d812ae8de4b51531064 Mon Sep 17 00:00:00 2001 From: Auguste MARBEC Date: Thu, 2 Jan 2025 16:45:38 +0100 Subject: [PATCH 15/28] modify self.check_inputs() --- boxmot/trackers/basetracker.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index ae97a62252..c7b6ab6c5d 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -182,13 +182,14 @@ def check_inputs(self, dets, img): ), "Unsupported 'dets' dimensions, valid number of dimensions is two" if self.is_obb : - assert ( - dets.shape[1] == 6 - ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6 (x1,y1,x2,y2,conf,cls)" - else : assert ( dets.shape[1] == 7 ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6 (cx,cy,w,h,angle,conf,cls)" + + else : + assert ( + dets.shape[1] == 6 + ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6 (x1,y1,x2,y2,conf,cls)" def id_to_color(self, id: int, saturation: float = 0.75, value: float = 0.95) -> tuple: From a705f56c9b91268839f14fd15aa0a88198cb37d9 Mon Sep 17 00:00:00 2001 From: Auguste MARBEC Date: Thu, 2 Jan 2025 16:48:11 +0100 Subject: [PATCH 16/28] correct shape to get column vector --- boxmot/trackers/ocsort/ocsort.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 55a47810fd..8dd034bf8f 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -205,7 +205,7 @@ def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.0 self.Q_xy_scaling = Q_xy_scaling self.Q_a_scaling = Q_a_scaling - self.kf = KalmanFilterXYSR(dim_x=10, dim_z=5, max_obs=max_obs) + self.kf = KalmanFilterXYWHA(dim_x=10, dim_z=5, max_obs=max_obs) self.kf.F = np.array( [ [1, 0, 0, 0, 0, 1, 0, 0, 0, 0], # cx = cx + vx @@ -239,7 +239,7 @@ def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.0 self.kf.Q[5:7, 5:7] *= self.Q_xy_scaling self.kf.Q[-1, -1] *= self.Q_a_scaling - self.kf.x[:5] = bbox + self.kf.x[:5] = bbox[:5].reshape((5, 1)) # x, y, w, h, angle (dont take confidence score) self.time_since_update = 0 self.id = KalmanBoxTrackerOBB.count KalmanBoxTrackerOBB.count += 1 @@ -295,7 +295,7 @@ def update(self, bbox, cls, det_ind): self.time_since_update = 0 self.hits += 1 self.hit_streak += 1 - self.kf.update(bbox) + self.kf.update(bbox[:5].reshape((5, 1))) # x, y, w, h, angle as column vector (dont take confidence score) else: self.kf.update(bbox) @@ -312,14 +312,14 @@ def predict(self): if self.time_since_update > 0: self.hit_streak = 0 self.time_since_update += 1 - self.history.append(self.kf.x) + self.history.append(self.kf.x[0:5].reshape((1, 5))) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ - return self.kf.x + return self.kf.x[0:5].reshape((1, 5)) class OcSort(BaseTracker): @@ -499,7 +499,10 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> # create and initialise new trackers for unmatched detections for i in unmatched_dets: - trk = KalmanBoxTracker(dets[i, :5+self.is_obb], dets[i, 5+self.is_obb], dets[i, 6+self.is_obb], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_s_scaling=self.Q_s_scaling, max_obs=self.max_obs) + if self.is_obb: + trk = KalmanBoxTrackerOBB(dets[i, :5+self.is_obb], dets[i, 5+self.is_obb], dets[i, 6+self.is_obb], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_a_scaling=self.Q_s_scaling, max_obs=self.max_obs) + else: + trk = KalmanBoxTracker(dets[i, :5], dets[i, 5], dets[i, 6], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_s_scaling=self.Q_s_scaling, max_obs=self.max_obs) self.active_tracks.append(trk) i = len(self.active_tracks) for trk in reversed(self.active_tracks): From 751242e09cab8b2a411f0fc7aa731695a32511a9 Mon Sep 17 00:00:00 2001 From: Auguste MARBEC Date: Thu, 2 Jan 2025 16:51:16 +0100 Subject: [PATCH 17/28] [-1, -1, -1, -1, -1] in k_previous_obs depends on is_obb, think about doing differently --- boxmot/trackers/ocsort/ocsort.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 8dd034bf8f..2d9275b893 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -14,9 +14,12 @@ from boxmot.utils.ops import xyxy2xysr -def k_previous_obs(observations, cur_age, k): +def k_previous_obs(observations, cur_age, k, is_obb=False): if len(observations) == 0: - return [-1, -1, -1, -1, -1] + if is_obb: + return [-1, -1, -1, -1, -1, -1] + else : + return [-1, -1, -1, -1, -1] for i in range(k): dt = k - i if cur_age - dt in observations: From 01eb615ac64e78c49622aba56594cef52e1a65a3 Mon Sep 17 00:00:00 2001 From: Auguste Verdier Date: Wed, 8 Jan 2025 17:26:33 +0100 Subject: [PATCH 18/28] overwrite plot function --- boxmot/trackers/ocsort/ocsort.py | 142 ++++++++++++++++++++++++++++++- 1 file changed, 140 insertions(+), 2 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 2d9275b893..f72ac8cafe 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -6,6 +6,7 @@ import numpy as np from collections import deque +import cv2 as cv from boxmot.motion.kalman_filters.xysr_kf import KalmanFilterXYSR from boxmot.motion.kalman_filters.xywha_kf import KalmanFilterXYWHA @@ -423,9 +424,10 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> ] ) last_boxes = np.array([trk.last_observation for trk in self.active_tracks]) + k_observations = np.array( [ - k_previous_obs(trk.observations, trk.age, self.delta_t) + k_previous_obs(trk.observations, trk.age, self.delta_t, is_obb=self.is_obb) for trk in self.active_tracks ] ) @@ -532,4 +534,140 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> self.active_tracks.pop(i) if len(ret) > 0: return np.concatenate(ret) - return np.array([]) \ No newline at end of file + return np.array([]) + + def plot_box_on_img(self, img: np.ndarray, box: tuple, conf: float, cls: int, id: int, thickness: int = 2, fontscale: float = 0.5) -> np.ndarray: + """ + Draws a bounding box with ID, confidence, and class information on an image. + + Parameters: + - img (np.ndarray): The image array to draw on. + - box (tuple): The bounding box coordinates as (x1, y1, x2, y2). + - conf (float): Confidence score of the detection. + - cls (int): Class ID of the detection. + - id (int): Unique identifier for the detection. + - thickness (int): The thickness of the bounding box. + - fontscale (float): The font scale for the text. + + Returns: + - np.ndarray: The image array with the bounding box drawn on it. + """ + if self.is_obb: + + angle = box[4] * 180.0 / np.pi # Convert radians to degrees + box_poly = ((box[0], box[1]), (box[2], box[3]), angle) + # print((width, height)) + rotrec = cv.boxPoints(box_poly) + box_poly = np.int_(rotrec) # Convert to integer + + # Draw the rectangle on the image + img = cv.polylines(img, [box_poly], isClosed=True, color=self.id_to_color(id), thickness=thickness) + + img = cv.putText( + img, + f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}, a: {box[4]:.2f}', + (int(box[0]), int(box[1]) - 10), + cv.FONT_HERSHEY_SIMPLEX, + fontscale, + self.id_to_color(id), + thickness + ) + else : + + img = cv.rectangle( + img, + (int(box[0]), int(box[1])), + (int(box[2]), int(box[3])), + self.id_to_color(id), + thickness + ) + img = cv.putText( + img, + f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}', + (int(box[0]), int(box[1]) - 10), + cv.FONT_HERSHEY_SIMPLEX, + fontscale, + self.id_to_color(id), + thickness + ) + return img + + + def plot_trackers_trajectories(self, img: np.ndarray, observations: list, id: int) -> np.ndarray: + """ + Draws the trajectories of tracked objects based on historical observations. Each point + in the trajectory is represented by a circle, with the thickness increasing for more + recent observations to visualize the path of movement. + + Parameters: + - img (np.ndarray): The image array on which to draw the trajectories. + - observations (list): A list of bounding box coordinates representing the historical + observations of a tracked object. Each observation is in the format (x1, y1, x2, y2). + - id (int): The unique identifier of the tracked object for color consistency in visualization. + + Returns: + - np.ndarray: The image array with the trajectories drawn on it. + """ + for i, box in enumerate(observations): + trajectory_thickness = int(np.sqrt(float (i + 1)) * 1.2) + if self.is_obb: + img = cv.circle( + img, + (int(box[0]), int(box[1])), + 2, + color=self.id_to_color(int(id)), + thickness=trajectory_thickness + ) + else: + + img = cv.circle( + img, + (int((box[0] + box[2]) / 2), + int((box[1] + box[3]) / 2)), + 2, + color=self.id_to_color(int(id)), + thickness=trajectory_thickness + ) + return img + + def plot_results(self, img: np.ndarray, show_trajectories: bool, thickness: int = 2, fontscale: float = 0.5) -> np.ndarray: + """ + Visualizes the trajectories of all active tracks on the image. For each track, + it draws the latest bounding box and the path of movement if the history of + observations is longer than two. This helps in understanding the movement patterns + of each tracked object. + + Parameters: + - img (np.ndarray): The image array on which to draw the trajectories and bounding boxes. + - show_trajectories (bool): Whether to show the trajectories. + - thickness (int): The thickness of the bounding box. + - fontscale (float): The font scale for the text. + + Returns: + - np.ndarray: The image array with trajectories and bounding boxes of all active tracks. + """ + + # if values in dict + if self.per_class_active_tracks is not None: + for k in self.per_class_active_tracks.keys(): + active_tracks = self.per_class_active_tracks[k] + for a in active_tracks: + if a.history_observations: + if a.hit_streak >= self.min_hits: + if len(a.history_observations) > 2: + box = a.history_observations[-1] + img = self.plot_box_on_img(img, box, a.conf, a.cls, a.id, thickness, fontscale) + if show_trajectories: + img = self.plot_trackers_trajectories(img, a.history_observations, a.id) + else: + for a in self.active_tracks: + if a.history_observations: + if a.hit_streak >= self.min_hits: + if len(a.history_observations) > 2: + box = a.history_observations[-1] + img = self.plot_box_on_img(img, box, a.conf, a.cls, a.id, thickness, fontscale) + if show_trajectories: + img = self.plot_trackers_trajectories(img, a.history_observations, a.id) + + return img + \ No newline at end of file From 8fdb8b484c83a7f3bd3b7a9d423d68a79d41a0a0 Mon Sep 17 00:00:00 2001 From: Auguste Verdier Date: Thu, 9 Jan 2025 15:02:52 +0100 Subject: [PATCH 19/28] plot only if hits > min_hit --- boxmot/trackers/ocsort/ocsort.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index f72ac8cafe..fd80c71d4a 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -653,7 +653,7 @@ def plot_results(self, img: np.ndarray, show_trajectories: bool, thickness: int active_tracks = self.per_class_active_tracks[k] for a in active_tracks: if a.history_observations: - if a.hit_streak >= self.min_hits: + if a.hits >= self.min_hits: if len(a.history_observations) > 2: box = a.history_observations[-1] img = self.plot_box_on_img(img, box, a.conf, a.cls, a.id, thickness, fontscale) @@ -662,7 +662,7 @@ def plot_results(self, img: np.ndarray, show_trajectories: bool, thickness: int else: for a in self.active_tracks: if a.history_observations: - if a.hit_streak >= self.min_hits: + if a.hits >= self.min_hits: if len(a.history_observations) > 2: box = a.history_observations[-1] img = self.plot_box_on_img(img, box, a.conf, a.cls, a.id, thickness, fontscale) From f01be73887d5ff4d8f9dce238218b4e735252ff5 Mon Sep 17 00:00:00 2001 From: Auguste Verdier Date: Mon, 13 Jan 2025 10:25:55 +0100 Subject: [PATCH 20/28] re index --- boxmot/trackers/ocsort/ocsort.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index fd80c71d4a..573d24df3d 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -439,7 +439,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> dets[:, 0:5+self.is_obb], trks, self.asso_func, self.asso_threshold, velocities, k_observations, self.inertia, w, h ) for m in matched: - self.active_tracks[m[1]].update(dets[m[0], :5+self.is_obb], dets[m[0], 5+self.is_obb], dets[m[0], 6+self.is_obb]) + self.active_tracks[m[1]].update(dets[m[0], :-2], dets[m[0], -2], dets[m[0], -1]) """ Second round of associaton by OCR @@ -464,7 +464,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> if iou_left[m[0], m[1]] < self.asso_threshold: continue self.active_tracks[trk_ind].update( - dets_second[det_ind, :5+self.is_obb], dets_second[det_ind, 5+self.is_obb], dets_second[det_ind, 6+self.is_obb] + dets_second[det_ind, :-2], dets_second[det_ind, -2], dets_second[det_ind, -1] ) to_remove_trk_indices.append(trk_ind) unmatched_trks = np.setdiff1d( @@ -489,7 +489,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> det_ind, trk_ind = unmatched_dets[m[0]], unmatched_trks[m[1]] if iou_left[m[0], m[1]] < self.asso_threshold: continue - self.active_tracks[trk_ind].update(dets[det_ind, :5+self.is_obb], dets[det_ind, 5+self.is_obb], dets[det_ind, 6+self.is_obb]) + self.active_tracks[trk_ind].update(dets[det_ind, :-2], dets[det_ind, -2], dets[det_ind, -1]) to_remove_det_indices.append(det_ind) to_remove_trk_indices.append(trk_ind) unmatched_dets = np.setdiff1d( @@ -505,7 +505,7 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> # create and initialise new trackers for unmatched detections for i in unmatched_dets: if self.is_obb: - trk = KalmanBoxTrackerOBB(dets[i, :5+self.is_obb], dets[i, 5+self.is_obb], dets[i, 6+self.is_obb], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_a_scaling=self.Q_s_scaling, max_obs=self.max_obs) + trk = KalmanBoxTrackerOBB(dets[i, :-2], dets[i, -2], dets[i, -1], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_a_scaling=self.Q_s_scaling, max_obs=self.max_obs) else: trk = KalmanBoxTracker(dets[i, :5], dets[i, 5], dets[i, 6], delta_t=self.delta_t, Q_xy_scaling=self.Q_xy_scaling, Q_s_scaling=self.Q_s_scaling, max_obs=self.max_obs) self.active_tracks.append(trk) From 860c6d99a250aeee33e969c2c7f6095ef839f39f Mon Sep 17 00:00:00 2001 From: Auguste Verdier Date: Tue, 14 Jan 2025 11:48:41 +0100 Subject: [PATCH 21/28] add iou_batch_obb --- boxmot/utils/iou.py | 43 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/boxmot/utils/iou.py b/boxmot/utils/iou.py index fdcb32188f..19fdcc9f7c 100644 --- a/boxmot/utils/iou.py +++ b/boxmot/utils/iou.py @@ -1,4 +1,35 @@ import numpy as np +import cv2 as cv + +def iou_obb_pair(i, j, bboxes1, bboxes2): + """ + Compute IoU for the rotated rectangles at index i and j in the batches `bboxes1`, `bboxes2` . + """ + rect1 = bboxes1[int(i)] + rect2 = bboxes2[int(j)] + + (cx1, cy1, w1, h1, angle1) = rect1[0:5] + (cx2, cy2, w2, h2, angle2) = rect2[0:5] + + + r1 = ((cx1, cy1), (w1, h1), angle1) + r2 = ((cx2, cy2), (w2, h2), angle2) + + # Compute intersection + ret, intersect = cv.rotatedRectangleIntersection(r1, r2) + if ret == 0 or intersect is None: + return 0.0 # No intersection + + # Calculate intersection area + intersection_area = cv.contourArea(intersect) + + # Calculate union area + area1 = w1 * h1 + area2 = w2 * h2 + union_area = area1 + area2 - intersection_area + + # Compute IoU + return intersection_area / union_area if union_area > 0 else 0.0 class AssociationFunction: def __init__(self, w, h, asso_mode="iou"): @@ -35,6 +66,17 @@ def iou_batch(bboxes1, bboxes2) -> np.ndarray: ) return o + @staticmethod + def iou_batch_obb(bboxes1, bboxes2) -> np.ndarray: + + N, M = len(bboxes1), len(bboxes2) + + def wrapper(i, j): + return iou_obb_pair(i, j, bboxes1, bboxes2) + + iou_matrix = np.fromfunction(np.vectorize(wrapper), shape=(N, M), dtype=int) + return iou_matrix + @staticmethod def hmiou_batch(bboxes1, bboxes2): """ @@ -291,6 +333,7 @@ def _get_asso_func(self, asso_mode): """ ASSO_FUNCS = { "iou": AssociationFunction.iou_batch, + "iou_obb": AssociationFunction.iou_batch_obb, "hmiou": AssociationFunction.hmiou_batch, "giou": AssociationFunction.giou_batch, "ciou": AssociationFunction.ciou_batch, From e0a117793244db8f7a69c1be4e59a745882cd973 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 10:09:47 +0100 Subject: [PATCH 22/28] enable obb tracking --- .../{ => aabb}/base_kalman_filter.py | 0 .../kalman_filters/{ => aabb}/xyah_kf.py | 2 +- .../kalman_filters/{ => aabb}/xysr_kf.py | 0 .../kalman_filters/{ => aabb}/xywh_kf.py | 2 +- boxmot/motion/kalman_filters/obb/xywha_kf.py | 637 ++++++++++++++++++ boxmot/motion/kalman_filters/xywha_kf.py | 468 ------------- boxmot/trackers/basetracker.py | 92 ++- boxmot/trackers/botsort/botsort.py | 4 +- boxmot/trackers/botsort/botsort_track.py | 2 +- boxmot/trackers/bytetrack/bytetrack.py | 4 +- boxmot/trackers/deepocsort/deepocsort.py | 5 +- boxmot/trackers/hybridsort/hybridsort.py | 2 +- boxmot/trackers/imprassoc/imprassoctrack.py | 4 +- boxmot/trackers/ocsort/ocsort.py | 443 +----------- boxmot/trackers/strongsort/sort/track.py | 2 +- boxmot/utils/selective_reid.py | 99 +++ 16 files changed, 820 insertions(+), 946 deletions(-) rename boxmot/motion/kalman_filters/{ => aabb}/base_kalman_filter.py (100%) rename boxmot/motion/kalman_filters/{ => aabb}/xyah_kf.py (97%) rename boxmot/motion/kalman_filters/{ => aabb}/xysr_kf.py (100%) rename boxmot/motion/kalman_filters/{ => aabb}/xywh_kf.py (96%) create mode 100644 boxmot/motion/kalman_filters/obb/xywha_kf.py delete mode 100644 boxmot/motion/kalman_filters/xywha_kf.py create mode 100644 boxmot/utils/selective_reid.py diff --git a/boxmot/motion/kalman_filters/base_kalman_filter.py b/boxmot/motion/kalman_filters/aabb/base_kalman_filter.py similarity index 100% rename from boxmot/motion/kalman_filters/base_kalman_filter.py rename to boxmot/motion/kalman_filters/aabb/base_kalman_filter.py diff --git a/boxmot/motion/kalman_filters/xyah_kf.py b/boxmot/motion/kalman_filters/aabb/xyah_kf.py similarity index 97% rename from boxmot/motion/kalman_filters/xyah_kf.py rename to boxmot/motion/kalman_filters/aabb/xyah_kf.py index 7db546aeca..63100669fd 100644 --- a/boxmot/motion/kalman_filters/xyah_kf.py +++ b/boxmot/motion/kalman_filters/aabb/xyah_kf.py @@ -1,6 +1,6 @@ import numpy as np from typing import Tuple -from boxmot.motion.kalman_filters.base_kalman_filter import BaseKalmanFilter +from boxmot.motion.kalman_filters.aabb.base_kalman_filter import BaseKalmanFilter class KalmanFilterXYAH(BaseKalmanFilter): diff --git a/boxmot/motion/kalman_filters/xysr_kf.py b/boxmot/motion/kalman_filters/aabb/xysr_kf.py similarity index 100% rename from boxmot/motion/kalman_filters/xysr_kf.py rename to boxmot/motion/kalman_filters/aabb/xysr_kf.py diff --git a/boxmot/motion/kalman_filters/xywh_kf.py b/boxmot/motion/kalman_filters/aabb/xywh_kf.py similarity index 96% rename from boxmot/motion/kalman_filters/xywh_kf.py rename to boxmot/motion/kalman_filters/aabb/xywh_kf.py index c0af94dda4..8c933ca45d 100644 --- a/boxmot/motion/kalman_filters/xywh_kf.py +++ b/boxmot/motion/kalman_filters/aabb/xywh_kf.py @@ -1,6 +1,6 @@ import numpy as np from typing import Tuple -from boxmot.motion.kalman_filters.base_kalman_filter import BaseKalmanFilter +from boxmot.motion.kalman_filters.aabb.base_kalman_filter import BaseKalmanFilter class KalmanFilterXYWH(BaseKalmanFilter): diff --git a/boxmot/motion/kalman_filters/obb/xywha_kf.py b/boxmot/motion/kalman_filters/obb/xywha_kf.py new file mode 100644 index 0000000000..c42f0adc6e --- /dev/null +++ b/boxmot/motion/kalman_filters/obb/xywha_kf.py @@ -0,0 +1,637 @@ +from __future__ import absolute_import, division + +from copy import deepcopy +from math import log, exp, sqrt, pi +import sys +import numpy as np +from numpy import dot, zeros, eye, isscalar +import numpy.linalg as linalg +from filterpy.stats import logpdf +from filterpy.common import pretty_str, reshape_z +from collections import deque + + +def speed_direction_obb(bbox1, bbox2): + cx1, cy1 = bbox1[0], bbox1[1] + cx2, cy2 = bbox2[0], bbox2[1] + speed = np.array([cy2 - cy1, cx2 - cx1]) + norm = np.sqrt((cy2 - cy1) ** 2 + (cx2 - cx1) ** 2) + 1e-6 + return speed / norm + + +class KalmanBoxTrackerOBB(object): + """ + This class represents the internal state of individual tracked objects observed as oriented bbox. + """ + + count = 0 + + def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_a_scaling = 0.01): + """ + Initialises a tracker using initial bounding box. + """ + # define constant velocity model + self.det_ind = det_ind + + self.Q_xy_scaling = Q_xy_scaling + self.Q_a_scaling = Q_a_scaling + + self.kf = KalmanFilterXYWHA(dim_x=10, dim_z=5, max_obs=max_obs) + self.kf.F = np.array( + [ + [1, 0, 0, 0, 0, 1, 0, 0, 0, 0], # cx = cx + vx + [0, 1, 0, 0, 0, 0, 1, 0, 0, 0], # cy = cy + vy + [0, 0, 1, 0, 0, 0, 0, 1, 0, 0], # w = w + vw + [0, 0, 0, 1, 0, 0, 0, 0, 1, 0], # h = h + vh + [0, 0, 0, 0, 1, 0, 0, 0, 0, 1], # a = a + va + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 1] + ] + ) + self.kf.H = np.array( + [ + [1, 0, 0, 0, 0, 0, 0, 0, 0 ,0], # cx + [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], # cy + [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], # w + [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], # h + [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], # angle + ] + ) + + self.kf.R[2:, 2:] *= 10.0 + self.kf.P[ + 5:, 5: + ] *= 1000.0 # give high uncertainty to the unobservable initial velocities + self.kf.P *= 10.0 + + self.kf.Q[5:7, 5:7] *= self.Q_xy_scaling + self.kf.Q[-1, -1] *= self.Q_a_scaling + + self.kf.x[:5] = bbox[:5].reshape((5, 1)) # x, y, w, h, angle (dont take confidence score) + self.time_since_update = 0 + self.id = KalmanBoxTrackerOBB.count + KalmanBoxTrackerOBB.count += 1 + self.max_obs = max_obs + self.history = deque([], maxlen=self.max_obs) + self.hits = 0 + self.hit_streak = 0 + self.age = 0 + self.conf = bbox[-1] + self.cls = cls + """ + NOTE: [-1,-1,-1,-1,-1] is a compromising placeholder for non-observation status, the same for the return of + function k_previous_obs. It is ugly and I do not like it. But to support generate observation array in a + fast and unified way, which you would see below k_observations = np.array([k_previous_obs(...]]), + let's bear it for now. + """ + self.last_observation = np.array([-1, -1, -1, -1, -1, -1]) #WARNING : -1 is a valid angle value + self.observations = dict() + self.history_observations = deque([], maxlen=self.max_obs) + self.velocity = None + self.delta_t = delta_t + + def update(self, bbox, cls, det_ind): + """ + Updates the state vector with observed bbox. + """ + self.det_ind = det_ind + if bbox is not None: + self.conf = bbox[-1] + self.cls = cls + if self.last_observation.sum() >= 0: # no previous observation + previous_box = None + for i in range(self.delta_t): + dt = self.delta_t - i + if self.age - dt in self.observations: + previous_box = self.observations[self.age - dt] + break + if previous_box is None: + previous_box = self.last_observation + """ + Estimate the track speed direction with observations \Delta t steps away + """ + self.velocity = speed_direction_obb(previous_box, bbox) + + """ + Insert new observations. This is a ugly way to maintain both self.observations + and self.history_observations. Bear it for the moment. + """ + self.last_observation = bbox + self.observations[self.age] = bbox + self.history_observations.append(bbox) + + self.time_since_update = 0 + self.hits += 1 + self.hit_streak += 1 + self.kf.update(bbox[:5].reshape((5, 1))) # x, y, w, h, angle as column vector (dont take confidence score) + else: + self.kf.update(bbox) + + def predict(self): + """ + Advances the state vector and returns the predicted bounding box estimate. + """ + if (self.kf.x[7] + self.kf.x[2]) <= 0: # Negative width + self.kf.x[7] *= 0.0 + if (self.kf.x[8] + self.kf.x[3]) <= 0: # Negative Height + self.kf.x[8] *= 0.0 + self.kf.predict() + self.age += 1 + if self.time_since_update > 0: + self.hit_streak = 0 + self.time_since_update += 1 + self.history.append(self.kf.x[0:5].reshape((1, 5))) + return self.history[-1] + + def get_state(self): + """ + Returns the current bounding box estimate. + """ + return self.kf.x[0:5].reshape((1, 5)) + + +class KalmanFilterXYWHA(object): + """ + Implements a Kalman Filter specialized for tracking Oriented Bounding Boxes. + The default state vector is [x, y, w, h, a]^T: + + - (x, y): center of the bounding box + - w, h : width and height of the bounding box + - a : orientation angle (radians) + + This filter supports "freeze" and "unfreeze" methods to handle missing + observations (no measurements) or out-of-sequence (OOS) smoothing logic. + """ + + def __init__(self, dim_x, dim_z, dim_u=0, max_obs=50): + """ + Parameters + ---------- + dim_x : int + Dimensionality of the state vector. Typically 5 if [x, y, w, h, a]. + dim_z : int + Dimensionality of the measurement vector. Typically also 5. + dim_u : int + Dimensionality of the control vector. Default is 0 (no control). + max_obs : int + Maximum number of stored observations for freeze/unfreeze logic. + """ + if dim_x < 1: + raise ValueError('dim_x must be 1 or greater') + if dim_z < 1: + raise ValueError('dim_z must be 1 or greater') + if dim_u < 0: + raise ValueError('dim_u must be 0 or greater') + + self.dim_x = dim_x + self.dim_z = dim_z + self.dim_u = dim_u + + # State: x is a (dim_x, 1) column vector + self.x = zeros((dim_x, 1)) # state + self.P = eye(dim_x) # covariance of the state + self.Q = eye(dim_x) # process noise covariance + self.B = None # control transition matrix + self.F = eye(dim_x) # state transition matrix + self.H = zeros((dim_z, dim_x)) # measurement function + self.R = eye(dim_z) # measurement noise covariance + self._alpha_sq = 1. # fading memory control + self.M = np.zeros((dim_x, dim_z)) # cross correlation (rarely used) + self.z = np.array([[None]*self.dim_z]).T + + # Gains and residuals computed during update + self.K = np.zeros((dim_x, dim_z)) # Kalman gain + self.y = zeros((dim_z, 1)) # residual + self.S = np.zeros((dim_z, dim_z)) # system uncertainty (innovation covariance) + self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty + + # Identity matrix (used in update) + self._I = np.eye(dim_x) + + # Save prior (after predict) and posterior (after update) + self.x_prior = self.x.copy() + self.P_prior = self.P.copy() + self.x_post = self.x.copy() + self.P_post = self.P.copy() + + # Internal log-likelihood computations + self._log_likelihood = log(sys.float_info.min) + self._likelihood = sys.float_info.min + self._mahalanobis = None + + # Store recent observations for freeze/unfreeze logic + self.max_obs = max_obs + self.history_obs = deque([], maxlen=self.max_obs) + + # For potential smoothing usage + self.inv = np.linalg.inv + self.attr_saved = None + self.observed = False + self.last_measurement = None + + def apply_affine_correction(self, m, t): + """ + Apply an affine transform to the current state and covariance. + This is useful if the image or reference frame is warped. + + Parameters + ---------- + m : np.array(2x2) + Affine transform (rotation/scale) to be applied to x,y and maybe w,h + t : np.array(2x1) + Translation vector to be added after applying the transform. + + TODO: adapt for oriented bounding box (especially if the orientation + is also changed by the transform). + """ + # For demonstration, we apply the transform to [x, y] and [x_dot, y_dot], etc. + # But for your OBB case, consider carefully how w, h, and angle should transform. + + # Example basic approach: transform x, y + self.x[:2] = m @ self.x[:2] + t + + # Possibly transform w, h. But if w,h are not purely length in x,y directions, + # you might have to do something more elaborate. For demonstration: + self.x[2:4] = np.abs(m @ self.x[2:4]) # naive approach: scale w,h + + # P block for positions: + self.P[:2, :2] = m @ self.P[:2, :2] @ m.T + # P block for widths/heights (again naive if we treat w,h as x,y scale): + self.P[2:4, 2:4] = m @ self.P[2:4, 2:4] @ m.T + + # If angle is included, consider adjusting it or leaving it if the transform + # is purely in the plane with no orientation offset. Could do angle wrap here. + + # If we froze the filter, also update the frozen state + if not self.observed and self.attr_saved is not None: + self.attr_saved["x"][:2] = m @ self.attr_saved["x"][:2] + t + self.attr_saved["x"][2:4] = np.abs(m @ self.attr_saved["x"][2:4]) + self.attr_saved["P"][:2, :2] = m @ self.attr_saved["P"][:2, :2] @ m.T + self.attr_saved["P"][2:4, 2:4] = m @ self.attr_saved["P"][2:4, 2:4] @ m.T + + # last_measurement might need updating similarly + self.attr_saved["last_measurement"][:2] = ( + m @ self.attr_saved["last_measurement"][:2] + t + ) + + def predict(self, u=None, B=None, F=None, Q=None): + """ + Predict next state (prior) using the state transition matrix F + and process noise Q. + + Parameters + ---------- + u : np.array(dim_u, 1), optional + Control vector. If not provided, assumed 0. + B : np.array(dim_x, dim_u), optional + Control transition matrix. If None, self.B is used. + F : np.array(dim_x, dim_x), optional + State transition matrix. If None, self.F is used. + Q : np.array(dim_x, dim_x) or scalar, optional + Process noise matrix. If None, self.Q is used. If scalar, + Q = scalar * I. + """ + if B is None: + B = self.B + if F is None: + F = self.F + if Q is None: + Q = self.Q + elif isscalar(Q): + Q = eye(self.dim_x) * Q + + # x = F x + B u + if B is not None and u is not None: + self.x = dot(F, self.x) + dot(B, u) + else: + self.x = dot(F, self.x) + + # P = F P F^T + Q + self.P = self._alpha_sq * dot(dot(F, self.P), F.T) + Q + + # Save the prior + self.x_prior = self.x.copy() + self.P_prior = self.P.copy() + + # ---- New: Enforce bounding box and angle constraints (if dim_x >= 5) ---- + if self.dim_x >= 5: + # clamp w, h > 0 + self.x[2, 0] = max(self.x[2, 0], 1e-4) + self.x[3, 0] = max(self.x[3, 0], 1e-4) + + # wrap angle to [-pi, pi] + self.x[4, 0] = (self.x[4, 0] + pi) % (2 * pi) - pi + + def freeze(self): + """ + Save the current filter parameters in attr_saved so that if the next + observation is missing, we can revert to these parameters for + out-of-sequence or offline smoothing. + """ + self.attr_saved = deepcopy(self.__dict__) + + def unfreeze(self): + """ + Revert the filter parameters to the saved (frozen) state, then "replay" + the missing measurements from history to smooth the intermediate states. + """ + if self.attr_saved is not None: + new_history = deepcopy(list(self.history_obs)) + # revert to the frozen attributes + self.__dict__ = self.attr_saved + # remove last measurement from history (since we'll re-apply them) + self.history_obs = deque(list(self.history_obs)[:-1], maxlen=self.max_obs) + + # naive approach: re-update states between the two known measurements + occur = [int(d is None) for d in new_history] + indices = np.where(np.array(occur) == 0)[0] + if len(indices) < 2: + return # not enough measurements to replay + + index1, index2 = indices[-2], indices[-1] + box1, box2 = new_history[index1], new_history[index2] + x1, y1, w1, h1, a1 = box1 + x2, y2, w2, h2, a2 = box2 + time_gap = index2 - index1 + dx, dy = (x2 - x1) / time_gap, (y2 - y1) / time_gap + dw, dh = (w2 - w1) / time_gap, (h2 - h1) / time_gap + da = (a2 - a1) / time_gap + + for i in range(index2 - index1): + x_ = x1 + (i + 1) * dx + y_ = y1 + (i + 1) * dy + w_ = w1 + (i + 1) * dw + h_ = h1 + (i + 1) * dh + a_ = a1 + (i + 1) * da + + new_box = np.array([x_, y_, w_, h_, a_]).reshape((5, 1)) + self.update(new_box) + if i != (index2 - index1 - 1): + self.predict() + self.history_obs.pop() + self.history_obs.pop() + + def update(self, z, R=None, H=None): + """ + Incorporate a new measurement z into the state estimate. + + Parameters + ---------- + z : np.array(dim_z, 1) + Measurement vector. If None, skip update step (missing measurement). + R : np.array(dim_z, dim_z), scalar, or None + Measurement noise matrix. If None, self.R is used. + H : np.array(dim_z, dim_x) or None + Measurement function. If None, self.H is used. + """ + # reset log-likelihood computations + self._log_likelihood = None + self._likelihood = None + self._mahalanobis = None + + # Save the observation (even if None) + self.history_obs.append(z) + + # If measurement is missing + if z is None: + if self.observed: + # freeze the current parameters for future potential smoothing + self.last_measurement = self.history_obs[-2] + self.freeze() + self.observed = False + self.z = np.array([[None] * self.dim_z]).T + self.x_post = self.x.copy() + self.P_post = self.P.copy() + self.y = zeros((self.dim_z, 1)) + return + + # If we haven't observed for a while, revert to the frozen state + if not self.observed: + self.unfreeze() + self.observed = True + + if R is None: + R = self.R + elif isscalar(R): + R = eye(self.dim_z) * R + + if H is None: + H = self.H + z = reshape_z(z, self.dim_z, self.x.ndim) + + # y = z - Hx (residual) + self.y = z - dot(H, self.x) + + PHT = dot(self.P, H.T) + self.S = dot(H, PHT) + R + self.SI = self.inv(self.S) + + # K = PHT * SI + self.K = PHT.dot(self.SI) + + # Optional gating (commented out): + # mahal_dist = float(self.y.T @ self.SI @ self.y) + # gating_threshold = 9.21 # e.g., chi-square with 2-5 dof + # if mahal_dist > gating_threshold: + # # Outlier measurement, skip or handle differently + # return + + # x = x + K y + self.x = self.x + dot(self.K, self.y) + + # P = (I - K H) P (I - K H)^T + K R K^T + I_KH = self._I - dot(self.K, H) + self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T) + + # Save measurement and posterior + self.z = deepcopy(z) + self.x_post = self.x.copy() + self.P_post = self.P.copy() + + # ---- New: Enforce bounding box and angle constraints (if dim_x >= 5) ---- + if self.dim_x >= 5: + # clamp w, h > 0 + self.x[2, 0] = max(self.x[2, 0], 1e-4) + self.x[3, 0] = max(self.x[3, 0], 1e-4) + + # wrap angle to [-pi, pi] + self.x[4, 0] = (self.x[4, 0] + pi) % (2 * pi) - pi + + def update_steadystate(self, z, H=None): + """ + Update using precomputed steady-state gain (K_steady_state) and + steady-state covariance P. Only x is updated here. + P remains unchanged. + """ + if z is None: + self.history_obs.append(z) + return + + if H is None: + H = self.H + + # residual + self.y = z - dot(H, self.x) + + # x = x + K_steady_state * y + self.x = self.x + dot(self.K_steady_state, self.y) + + # Save measurement and posterior + self.z = deepcopy(z) + self.x_post = self.x.copy() + + self.history_obs.append(z) + + def log_likelihood_of(self, z=None): + """ + Compute the log-likelihood of measurement z given the current + measurement prediction. This uses logpdf from filterpy.stats. + """ + if z is None: + z = self.z + return logpdf(z, dot(self.H, self.x), self.S) + + def likelihood_of(self, z=None): + """ + Compute the likelihood (probability) of measurement z given + the current measurement prediction. + """ + return exp(self.log_likelihood_of(z)) + + @property + def log_likelihood(self): + """ log-likelihood of the last measurement. """ + return self._log_likelihood + + @property + def likelihood(self): + """ likelihood of the last measurement. """ + return self._likelihood + + +def batch_filter(x, P, zs, Fs, Qs, Hs, Rs, Bs=None, us=None, + update_first=False, saver=None): + """ + Batch processes a sequence of measurements. + + Parameters + ---------- + x : np.array(dim_x, 1) + Initial state. + P : np.array(dim_x, dim_x) + Initial covariance. + zs : list-like + List of measurements at each time step (None for missing). + Fs : list-like + State transition matrices for each step. + Qs : list-like + Process noise covariances for each step. + Hs : list-like + Measurement matrices for each step. + Rs : list-like + Measurement noise covariances for each step. + Bs : list-like, optional + Control transition matrices for each step. + us : list-like, optional + Control vectors for each step. + update_first : bool + If True, apply update->predict. Otherwise predict->update. + saver : filterpy.common.Saver, optional + If provided, saver.save() is called at each step. + + Returns + ------- + means : np.array((n,dim_x,1)) + covariances : np.array((n,dim_x,dim_x)) + means_p : np.array((n,dim_x,1)) + Predictions after each step + covariances_p : np.array((n,dim_x,dim_x)) + Covariances after prediction each step + """ + n = np.size(zs, 0) + dim_x = x.shape[0] + + # Arrays to store results + if x.ndim == 1: + means = np.zeros((n, dim_x)) + means_p = np.zeros((n, dim_x)) + else: + means = np.zeros((n, dim_x, 1)) + means_p = np.zeros((n, dim_x, 1)) + + covariances = np.zeros((n, dim_x, dim_x)) + covariances_p = np.zeros((n, dim_x, dim_x)) + + if us is None: + us = [0.0] * n + Bs = [0.0] * n + + # Procedural version of predict->update or update->predict + for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): + + if update_first: + # Update step + x, P = update(x, P, z, R=R, H=H) + means[i, :] = x + covariances[i, :, :] = P + + # Predict step + x, P = predict(x, P, u=u, B=B, F=F, Q=Q) + means_p[i, :] = x + covariances_p[i, :, :] = P + + else: + # Predict step + x, P = predict(x, P, u=u, B=B, F=F, Q=Q) + means_p[i, :] = x + covariances_p[i, :, :] = P + + # Update step + x, P = update(x, P, z, R=R, H=H) + means[i, :] = x + covariances[i, :, :] = P + + if saver is not None: + saver.save() + + return (means, covariances, means_p, covariances_p) + + +def update(x, P, z, R, H): + """ + Procedural form of the update step of the Kalman Filter. + """ + if z is None: + return x, P + + # y = z - Hx + y = z - dot(H, x) + PHT = dot(P, H.T) + S = dot(H, PHT) + R + SI = linalg.inv(S) + K = dot(PHT, SI) + + # x = x + Ky + x = x + dot(K, y) + + # P = (I - KH)P(I - KH)' + KRK' + I_KH = np.eye(x.shape[0]) - dot(K, H) + P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T) + + return x, P + + +def predict(x, P, F, Q, B=None, u=None): + """ + Procedural form of the predict step of the Kalman Filter. + """ + if B is not None and u is not None: + x = dot(F, x) + dot(B, u) + else: + x = dot(F, x) + + P = dot(dot(F, P), F.T) + Q + return x, P diff --git a/boxmot/motion/kalman_filters/xywha_kf.py b/boxmot/motion/kalman_filters/xywha_kf.py deleted file mode 100644 index 73d1b04d5f..0000000000 --- a/boxmot/motion/kalman_filters/xywha_kf.py +++ /dev/null @@ -1,468 +0,0 @@ -""" -This module implements the linear Kalman filter in both an object -oriented and procedural form. The KalmanFilter class implements -the filter by storing the various matrices in instance variables, -minimizing the amount of bookkeeping you have to do. -All Kalman filters operate with a predict->update cycle. The -predict step, implemented with the method or function predict(), -uses the state transition matrix F to predict the state in the next -time period (epoch). The state is stored as a gaussian (x, P), where -x is the state (column) vector, and P is its covariance. Covariance -matrix Q specifies the process covariance. In Bayesian terms, this -prediction is called the *prior*, which you can think of colloquially -as the estimate prior to incorporating the measurement. -The update step, implemented with the method or function `update()`, -incorporates the measurement z with covariance R, into the state -estimate (x, P). The class stores the system uncertainty in S, -the innovation (residual between prediction and measurement in -measurement space) in y, and the Kalman gain in k. The procedural -form returns these variables to you. In Bayesian terms this computes -the *posterior* - the estimate after the information from the -measurement is incorporated. -Whether you use the OO form or procedural form is up to you. If -matrices such as H, R, and F are changing each epoch, you'll probably -opt to use the procedural form. If they are unchanging, the OO -form is perhaps easier to use since you won't need to keep track -of these matrices. This is especially useful if you are implementing -banks of filters or comparing various KF designs for performance; -a trivial coding bug could lead to using the wrong sets of matrices. -This module also offers an implementation of the RTS smoother, and -other helper functions, such as log likelihood computations. -The Saver class allows you to easily save the state of the -KalmanFilter class after every update. -""" - -from __future__ import absolute_import, division - -from copy import deepcopy -from math import log, exp, sqrt -import sys -import numpy as np -from numpy import dot, zeros, eye, isscalar, shape -import numpy.linalg as linalg -from filterpy.stats import logpdf -from filterpy.common import pretty_str, reshape_z -from collections import deque - - -class KalmanFilterXYWHA(object): - """ Implements a Kalman filter. You are responsible for setting the - various state variables to reasonable values; the defaults will - not give you a functional filter. - """ - - def __init__(self, dim_x, dim_z, dim_u=0, max_obs=50): - if dim_x < 1: - raise ValueError('dim_x must be 1 or greater') - if dim_z < 1: - raise ValueError('dim_z must be 1 or greater') - if dim_u < 0: - raise ValueError('dim_u must be 0 or greater') - - self.dim_x = dim_x - self.dim_z = dim_z - self.dim_u = dim_u - - self.x = zeros((dim_x, 1)) # state - self.P = eye(dim_x) # uncertainty covariance - self.Q = eye(dim_x) # process uncertainty - self.B = None # control transition matrix - self.F = eye(dim_x) # state transition matrix - self.H = zeros((dim_z, dim_x)) # measurement function - self.R = eye(dim_z) # measurement uncertainty - self._alpha_sq = 1. # fading memory control - self.M = np.zeros((dim_x, dim_z)) # process-measurement cross correlation - self.z = np.array([[None]*self.dim_z]).T - - # gain and residual are computed during the innovation step. We - # save them so that in case you want to inspect them for various - # purposes - self.K = np.zeros((dim_x, dim_z)) # kalman gain - self.y = zeros((dim_z, 1)) - self.S = np.zeros((dim_z, dim_z)) # system uncertainty - self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty - - # identity matrix. Do not alter this. - self._I = np.eye(dim_x) - - # these will always be a copy of x,P after predict() is called - self.x_prior = self.x.copy() - self.P_prior = self.P.copy() - - # these will always be a copy of x,P after update() is called - self.x_post = self.x.copy() - self.P_post = self.P.copy() - - # Only computed only if requested via property - self._log_likelihood = log(sys.float_info.min) - self._likelihood = sys.float_info.min - self._mahalanobis = None - - # keep all observations - self.max_obs = max_obs - self.history_obs = deque([], maxlen=self.max_obs) - - self.inv = np.linalg.inv - - self.attr_saved = None - self.observed = False - self.last_measurement = None - - - def apply_affine_correction(self, m, t): - #TODO ADAPT FOR OBB - """ - Apply to both last state and last observation for OOS smoothing. - - Messy due to internal logic for kalman filter being messy. - """ - - scale = np.linalg.norm(m[:, 0]) - self.x[:2] = m @ self.x[:2] + t - self.x[4:6] = m @ self.x[4:6] - - self.P[:2, :2] = m @ self.P[:2, :2] @ m.T - self.P[4:6, 4:6] = m @ self.P[4:6, 4:6] @ m.T - - # If frozen, also need to update the frozen state for OOS - if not self.observed and self.attr_saved is not None: - self.attr_saved["x"][:2] = m @ self.attr_saved["x"][:2] + t - self.attr_saved["x"][4:6] = m @ self.attr_saved["x"][4:6] - - self.attr_saved["P"][:2, :2] = m @ self.attr_saved["P"][:2, :2] @ m.T - self.attr_saved["P"][4:6, 4:6] = m @ self.attr_saved["P"][4:6, 4:6] @ m.T - - self.attr_saved["last_measurement"][:2] = m @ self.attr_saved["last_measurement"][:2] + t - - - def predict(self, u=None, B=None, F=None, Q=None): - """ - Predict next state (prior) using the Kalman filter state propagation - equations. - Parameters - ---------- - u : np.array, default 0 - Optional control vector. - B : np.array(dim_x, dim_u), or None - Optional control transition matrix; a value of None - will cause the filter to use `self.B`. - F : np.array(dim_x, dim_x), or None - Optional state transition matrix; a value of None - will cause the filter to use `self.F`. - Q : np.array(dim_x, dim_x), scalar, or None - Optional process noise matrix; a value of None will cause the - filter to use `self.Q`. - """ - if B is None: - B = self.B - if F is None: - F = self.F - if Q is None: - Q = self.Q - elif isscalar(Q): - Q = eye(self.dim_x) * Q - - # x = Fx + Bu - if B is not None and u is not None: - self.x = dot(F, self.x) + dot(B, u) - else: - self.x = dot(F, self.x) - - # P = FPF' + Q - self.P = self._alpha_sq * dot(dot(F, self.P), F.T) + Q - - # save prior - self.x_prior = self.x.copy() - self.P_prior = self.P.copy() - - def freeze(self): - """ - Save the parameters before non-observation forward - """ - self.attr_saved = deepcopy(self.__dict__) - - def unfreeze(self): - if self.attr_saved is not None: - new_history = deepcopy(list(self.history_obs)) - self.__dict__ = self.attr_saved - self.history_obs = deque(list(self.history_obs)[:-1], maxlen=self.max_obs) - occur = [int(d is None) for d in new_history] - indices = np.where(np.array(occur) == 0)[0] - index1, index2 = indices[-2], indices[-1] - box1, box2 = new_history[index1], new_history[index2] - x1, y1, w1, h1, a1 = box1 - x2, y2, w2, h2, a2 = box2 - time_gap = index2 - index1 - dx, dy = (x2 - x1) / time_gap, (y2 - y1) / time_gap - dw, dh = (w2 - w1) / time_gap, (h2 - h1) / time_gap - da = (a2 - a1) / time_gap - for i in range(index2 - index1): - x, y = x1 + (i + 1) * dx, y1 + (i + 1) * dy - w, h = w1 + (i + 1) * dw, h1 + (i + 1) * dh - a = a1 + (i + 1) * da - new_box = np.array([x, y, w, h, a]).reshape((5, 1)) - self.update(new_box) - if not i == (index2 - index1 - 1): - self.predict() - self.history_obs.pop() - self.history_obs.pop() - - def update(self, z, R=None, H=None): - """ - Add a new measurement (z) to the Kalman filter. If z is None, nothing is changed. - Parameters - ---------- - z : np.array - Measurement for this update. z can be a scalar if dim_z is 1, - otherwise it must be a column vector. - R : np.array, scalar, or None - Measurement noise. If None, the filter's self.R value is used. - H : np.array, or None - Measurement function. If None, the filter's self.H value is used. - """ - - # set to None to force recompute - self._log_likelihood = None - self._likelihood = None - self._mahalanobis = None - - # append the observation - self.history_obs.append(z) - - if z is None: - if self.observed: - """ - Got no observation so freeze the current parameters for future - potential online smoothing. - """ - self.last_measurement = self.history_obs[-2] - self.freeze() - self.observed = False - self.z = np.array([[None] * self.dim_z]).T - self.x_post = self.x.copy() - self.P_post = self.P.copy() - self.y = zeros((self.dim_z, 1)) - return - - # self.observed = True - if not self.observed: - """ - Get observation, use online smoothing to re-update parameters - """ - self.unfreeze() - self.observed = True - - if R is None: - R = self.R - elif isscalar(R): - R = eye(self.dim_z) * R - if H is None: - z = reshape_z(z, self.dim_z, self.x.ndim) - H = self.H - - # y = z - Hx - # error (residual) between measurement and prediction - self.y = z - dot(H, self.x) - - # common subexpression for speed - PHT = dot(self.P, H.T) - - # S = HPH' + R - self.S = dot(H, PHT) + R - self.SI = self.inv(self.S) - - # K = PH'inv(S) - self.K = PHT.dot(self.SI) - - # x = x + Ky - self.x = self.x + dot(self.K, self.y) - - # P = (I-KH)P(I-KH)' + KRK' - I_KH = self._I - dot(self.K, H) - self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T) - - # save measurement and posterior state - self.z = deepcopy(z) - self.x_post = self.x.copy() - self.P_post = self.P.copy() - - # save history of observations - self.history_obs.append(z) - - def update_steadystate(self, z, H=None): - """ Update Kalman filter using the Kalman gain and state covariance - matrix as computed for the steady state. Only x is updated, and the - new value is stored in self.x. P is left unchanged. Must be called - after a prior call to compute_steady_state(). - """ - if z is None: - self.history_obs.append(z) - return - - if H is None: - H = self.H - - H = np.asarray(H) - # error (residual) between measurement and prediction - self.y = z - dot(H, self.x) - - # x = x + Ky - self.x = self.x + dot(self.K_steady_state, self.y) - - # save measurement and posterior state - self.z = deepcopy(z) - self.x_post = self.x.copy() - - # save history of observations - self.history_obs.append(z) - - def log_likelihood(self, z=None): - """ log-likelihood of the measurement z. Computed from the - system uncertainty S. - """ - - if z is None: - z = self.z - return logpdf(z, dot(self.H, self.x), self.S) - - def likelihood(self, z=None): - """ likelihood of the measurement z. Computed from the - system uncertainty S. - """ - - if z is None: - z = self.z - return exp(self.log_likelihood(z)) - - @property - def log_likelihood(self): - """ log-likelihood of the last measurement. - """ - - return self._log_likelihood - - @property - def likelihood(self): - """ likelihood of the last measurement. - """ - - return self._likelihood - - -def batch_filter(x, P, zs, Fs, Qs, Hs, Rs, Bs=None, us=None, update_first=False, saver=None): - """ - Batch processes a sequences of measurements. - Parameters - ---------- - zs : list-like - list of measurements at each time step. Missing measurements must be - represented by None. - Fs : list-like - list of values to use for the state transition matrix matrix. - Qs : list-like - list of values to use for the process error - covariance. - Hs : list-like - list of values to use for the measurement matrix. - Rs : list-like - list of values to use for the measurement error - covariance. - Bs : list-like, optional - list of values to use for the control transition matrix; - a value of None in any position will cause the filter - to use `self.B` for that time step. - us : list-like, optional - list of values to use for the control input vector; - a value of None in any position will cause the filter to use - 0 for that time step. - update_first : bool, optional - controls whether the order of operations is update followed by - predict, or predict followed by update. Default is predict->update. - saver : filterpy.common.Saver, optional - filterpy.common.Saver object. If provided, saver.save() will be - called after every epoch - Returns - ------- - means : np.array((n,dim_x,1)) - array of the state for each time step after the update. Each entry - is an np.array. In other words `means[k,:]` is the state at step - `k`. - covariance : np.array((n,dim_x,dim_x)) - array of the covariances for each time step after the update. - In other words `covariance[k,:,:]` is the covariance at step `k`. - means_predictions : np.array((n,dim_x,1)) - array of the state for each time step after the predictions. Each - entry is an np.array. In other words `means[k,:]` is the state at - step `k`. - covariance_predictions : np.array((n,dim_x,dim_x)) - array of the covariances for each time step after the prediction. - In other words `covariance[k,:,:]` is the covariance at step `k`. - Examples - -------- - .. code-block:: Python - zs = [t + random.randn()*4 for t in range (40)] - Fs = [kf.F for t in range (40)] - Hs = [kf.H for t in range (40)] - (mu, cov, _, _) = kf.batch_filter(zs, Rs=R_list, Fs=Fs, Hs=Hs, Qs=None, - Bs=None, us=None, update_first=False) - (xs, Ps, Ks, Pps) = kf.rts_smoother(mu, cov, Fs=Fs, Qs=None) - """ - - n = np.size(zs, 0) - dim_x = x.shape[0] - - # mean estimates from Kalman Filter - if x.ndim == 1: - means = zeros((n, dim_x)) - means_p = zeros((n, dim_x)) - else: - means = zeros((n, dim_x, 1)) - means_p = zeros((n, dim_x, 1)) - - # state covariances from Kalman Filter - covariances = zeros((n, dim_x, dim_x)) - covariances_p = zeros((n, dim_x, dim_x)) - - if us is None: - us = [0.0] * n - Bs = [0.0] * n - - if update_first: - for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): - - x, P = update(x, P, z, R=R, H=H) - means[i, :] = x - covariances[i, :, :] = P - - x, P = predict(x, P, u=u, B=B, F=F, Q=Q) - means_p[i, :] = x - covariances_p[i, :, :] = P - if saver is not None: - saver.save() - else: - for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): - - x, P = predict(x, P, u=u, B=B, F=F, Q=Q) - means_p[i, :] = x - covariances_p[i, :, :] = P - - x, P = update(x, P, z, R=R, H=H) - means[i, :] = x - covariances[i, :, :] = P - if saver is not None: - saver.save() - - return (means, covariances, means_p, covariances_p) - - def batch_filter(self, zs, Rs=None): - """ - Batch process a sequence of measurements. This method is suitable - for cases where the measurement noise varies with each measurement. - """ - means, covariances = [], [] - for z, R in zip(zs, Rs): - self.predict() - self.update(z, R=R) - means.append(self.x.copy()) - covariances.append(self.P.copy()) - return np.array(means), np.array(covariances) \ No newline at end of file diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index c7b6ab6c5d..a8b8dbc096 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -49,6 +49,7 @@ def __init__( self.active_tracks = [] # This might be handled differently in derived classes self.per_class_active_tracks = None self._first_frame_processed = False # Flag to track if the first frame has been processed + self._first_dets_processed = False # Initialize per-class active tracks if self.per_class: @@ -99,7 +100,7 @@ def get_class_dets_n_embs(self, dets, embs, cls_id): return class_dets, class_embs @staticmethod - def on_first_frame_setup(method): + def setup_decorator(method): """ Decorator to perform setup on the first frame only. This ensures that initialization tasks (like setting the association function) only @@ -107,6 +108,16 @@ def on_first_frame_setup(method): """ def wrapper(self, *args, **kwargs): # If setup hasn't been done yet, perform it + # Even if dets is empty (e.g., shape (0, 7)), this check will still pass if it's Nx7 + if not self._first_dets_processed: + dets = args[0] + if dets.ndim == 2 and dets.shape[1] == 6: + self.is_obb = False + self._first_dets_processed = True + elif dets.ndim == 2 and dets.shape[1] == 7: + self.is_obb = True + self._first_dets_processed = True + if not self._first_frame_processed: img = args[1] self.h, self.w = img.shape[0:2] @@ -243,23 +254,44 @@ def plot_box_on_img(self, img: np.ndarray, box: tuple, conf: float, cls: int, id Returns: - np.ndarray: The image array with the bounding box drawn on it. """ + if self.is_obb: + + angle = box[4] * 180.0 / np.pi # Convert radians to degrees + box_poly = ((box[0], box[1]), (box[2], box[3]), angle) + # print((width, height)) + rotrec = cv.boxPoints(box_poly) + box_poly = np.int_(rotrec) # Convert to integer + + # Draw the rectangle on the image + img = cv.polylines(img, [box_poly], isClosed=True, color=self.id_to_color(id), thickness=thickness) + + img = cv.putText( + img, + f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}, a: {box[4]:.2f}', + (int(box[0]), int(box[1]) - 10), + cv.FONT_HERSHEY_SIMPLEX, + fontscale, + self.id_to_color(id), + thickness + ) + else : - img = cv.rectangle( - img, - (int(box[0]), int(box[1])), - (int(box[2]), int(box[3])), - self.id_to_color(id), - thickness - ) - img = cv.putText( - img, - f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}', - (int(box[0]), int(box[1]) - 10), - cv.FONT_HERSHEY_SIMPLEX, - fontscale, - self.id_to_color(id), - thickness - ) + img = cv.rectangle( + img, + (int(box[0]), int(box[1])), + (int(box[2]), int(box[3])), + self.id_to_color(id), + thickness + ) + img = cv.putText( + img, + f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}', + (int(box[0]), int(box[1]) - 10), + cv.FONT_HERSHEY_SIMPLEX, + fontscale, + self.id_to_color(id), + thickness + ) return img @@ -280,14 +312,24 @@ def plot_trackers_trajectories(self, img: np.ndarray, observations: list, id: in """ for i, box in enumerate(observations): trajectory_thickness = int(np.sqrt(float (i + 1)) * 1.2) - img = cv.circle( - img, - (int((box[0] + box[2]) / 2), - int((box[1] + box[3]) / 2)), - 2, - color=self.id_to_color(int(id)), - thickness=trajectory_thickness - ) + if self.is_obb: + img = cv.circle( + img, + (int(box[0]), int(box[1])), + 2, + color=self.id_to_color(int(id)), + thickness=trajectory_thickness + ) + else: + + img = cv.circle( + img, + (int((box[0] + box[2]) / 2), + int((box[1] + box[3]) / 2)), + 2, + color=self.id_to_color(int(id)), + thickness=trajectory_thickness + ) return img diff --git a/boxmot/trackers/botsort/botsort.py b/boxmot/trackers/botsort/botsort.py index 3601d47a09..476f39b5a6 100644 --- a/boxmot/trackers/botsort/botsort.py +++ b/boxmot/trackers/botsort/botsort.py @@ -4,7 +4,7 @@ import numpy as np from pathlib import Path -from boxmot.motion.kalman_filters.xywh_kf import KalmanFilterXYWH +from boxmot.motion.kalman_filters.aabb.xywh_kf import KalmanFilterXYWH from boxmot.appearance.reid_auto_backend import ReidAutoBackend from boxmot.motion.cmc.sof import SOF from boxmot.trackers.botsort.basetrack import BaseTrack, TrackState @@ -84,7 +84,7 @@ def __init__( self.cmc = get_cmc_method(cmc_method)() self.fuse_first_associate = fuse_first_associate - @BaseTracker.on_first_frame_setup + @BaseTracker.setup_decorator @BaseTracker.per_class_decorator def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> np.ndarray: self.check_inputs(dets, img) diff --git a/boxmot/trackers/botsort/botsort_track.py b/boxmot/trackers/botsort/botsort_track.py index da7dcdfe2a..9f97b9f9bd 100644 --- a/boxmot/trackers/botsort/botsort_track.py +++ b/boxmot/trackers/botsort/botsort_track.py @@ -2,7 +2,7 @@ from collections import deque from boxmot.trackers.botsort.basetrack import BaseTrack, TrackState -from boxmot.motion.kalman_filters.xywh_kf import KalmanFilterXYWH +from boxmot.motion.kalman_filters.aabb.xywh_kf import KalmanFilterXYWH from boxmot.utils.ops import xywh2xyxy, xyxy2xywh diff --git a/boxmot/trackers/bytetrack/bytetrack.py b/boxmot/trackers/bytetrack/bytetrack.py index a3357327ce..e1d5ff8641 100644 --- a/boxmot/trackers/bytetrack/bytetrack.py +++ b/boxmot/trackers/bytetrack/bytetrack.py @@ -3,7 +3,7 @@ import numpy as np from collections import deque -from boxmot.motion.kalman_filters.xyah_kf import KalmanFilterXYAH +from boxmot.motion.kalman_filters.aabb.xyah_kf import KalmanFilterXYAH from boxmot.trackers.bytetrack.basetrack import BaseTrack, TrackState from boxmot.utils.matching import fuse_score, iou_distance, linear_assignment from boxmot.utils.ops import tlwh2xyah, xywh2tlwh, xywh2xyxy, xyxy2xywh @@ -150,7 +150,7 @@ def __init__( self.max_time_lost = self.buffer_size self.kalman_filter = KalmanFilterXYAH() - @BaseTracker.on_first_frame_setup + @BaseTracker.setup_decorator @BaseTracker.per_class_decorator def update(self, dets: np.ndarray, img: np.ndarray = None, embs: np.ndarray = None) -> np.ndarray: diff --git a/boxmot/trackers/deepocsort/deepocsort.py b/boxmot/trackers/deepocsort/deepocsort.py index 158a233582..95e069dc35 100644 --- a/boxmot/trackers/deepocsort/deepocsort.py +++ b/boxmot/trackers/deepocsort/deepocsort.py @@ -7,8 +7,7 @@ from boxmot.appearance.reid_auto_backend import ReidAutoBackend from boxmot.motion.cmc import get_cmc_method -from boxmot.motion.kalman_filters.xysr_kf import KalmanFilterXYSR -from boxmot.motion.kalman_filters.xywh_kf import KalmanFilterXYWH +from boxmot.motion.kalman_filters.aabb.xysr_kf import KalmanFilterXYSR from boxmot.utils.association import associate, linear_assignment from boxmot.trackers.basetracker import BaseTracker from boxmot.utils.ops import xyxy2xysr @@ -299,7 +298,7 @@ def __init__( self.cmc_off = cmc_off self.aw_off = aw_off - @BaseTracker.on_first_frame_setup + @BaseTracker.setup_decorator @BaseTracker.per_class_decorator def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> np.ndarray: """ diff --git a/boxmot/trackers/hybridsort/hybridsort.py b/boxmot/trackers/hybridsort/hybridsort.py index ed52af1217..144080e001 100644 --- a/boxmot/trackers/hybridsort/hybridsort.py +++ b/boxmot/trackers/hybridsort/hybridsort.py @@ -392,7 +392,7 @@ def camera_update(self, trackers, warp_matrix): for tracker in trackers: tracker.camera_update(warp_matrix) - @BaseTracker.on_first_frame_setup + @BaseTracker.setup_decorator @BaseTracker.per_class_decorator def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> np.ndarray: """ diff --git a/boxmot/trackers/imprassoc/imprassoctrack.py b/boxmot/trackers/imprassoc/imprassoctrack.py index 3bff68fda0..7f5a4b20ba 100644 --- a/boxmot/trackers/imprassoc/imprassoctrack.py +++ b/boxmot/trackers/imprassoc/imprassoctrack.py @@ -7,7 +7,7 @@ from boxmot.appearance.reid_auto_backend import ReidAutoBackend from boxmot.motion.cmc.sof import SOF -from boxmot.motion.kalman_filters.xywh_kf import KalmanFilterXYWH +from boxmot.motion.kalman_filters.aabb.xywh_kf import KalmanFilterXYWH from boxmot.trackers.imprassoc.basetrack import BaseTrack, TrackState from boxmot.utils.matching import (embedding_distance, fuse_score, iou_distance, linear_assignment, @@ -264,7 +264,7 @@ def __init__( self.cmc = SOF() - @BaseTracker.on_first_frame_setup + @BaseTracker.setup_decorator @BaseTracker.per_class_decorator def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> np.ndarray: self.check_inputs(dets, img) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index 573d24df3d..f732b48dad 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -8,11 +8,10 @@ import cv2 as cv -from boxmot.motion.kalman_filters.xysr_kf import KalmanFilterXYSR -from boxmot.motion.kalman_filters.xywha_kf import KalmanFilterXYWHA from boxmot.utils.association import associate, linear_assignment from boxmot.trackers.basetracker import BaseTracker from boxmot.utils.ops import xyxy2xysr +from boxmot.motion.kalman_filters.obb.xywha_kf import KalmanBoxTrackerOBB def k_previous_obs(observations, cur_age, k, is_obb=False): @@ -29,303 +28,6 @@ def k_previous_obs(observations, cur_age, k, is_obb=False): return observations[max_age] -def convert_x_to_bbox(x, score=None): - """ - Takes a bounding box in the centre form [x,y,s,r] and returns it in the form - [x1,y1,x2,y2] where x1,y1 is the top left and x2,y2 is the bottom right - """ - w = np.sqrt(x[2] * x[3]) - h = x[2] / w - if score is None: - return np.array( - [x[0] - w / 2.0, x[1] - h / 2.0, x[0] + w / 2.0, x[1] + h / 2.0] - ).reshape((1, 4)) - else: - return np.array( - [x[0] - w / 2.0, x[1] - h / 2.0, x[0] + w / 2.0, x[1] + h / 2.0, score] - ).reshape((1, 5)) - - -def speed_direction(bbox1, bbox2): - cx1, cy1 = (bbox1[0] + bbox1[2]) / 2.0, (bbox1[1] + bbox1[3]) / 2.0 - cx2, cy2 = (bbox2[0] + bbox2[2]) / 2.0, (bbox2[1] + bbox2[3]) / 2.0 - speed = np.array([cy2 - cy1, cx2 - cx1]) - norm = np.sqrt((cy2 - cy1) ** 2 + (cx2 - cx1) ** 2) + 1e-6 - return speed / norm - -def speed_direction_obb(bbox1, bbox2): - cx1, cy1 = bbox1[0], bbox1[1] - cx2, cy2 = bbox2[0], bbox2[1] - speed = np.array([cy2 - cy1, cx2 - cx1]) - norm = np.sqrt((cy2 - cy1) ** 2 + (cx2 - cx1) ** 2) + 1e-6 - return speed / norm - - -class KalmanBoxTracker(object): - """ - This class represents the internal state of individual tracked objects observed as bbox. - """ - - count = 0 - - def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_s_scaling = 0.0001): - """ - Initialises a tracker using initial bounding box. - - """ - # define constant velocity model - self.det_ind = det_ind - - self.Q_xy_scaling = Q_xy_scaling - self.Q_s_scaling = Q_s_scaling - - self.kf = KalmanFilterXYSR(dim_x=7, dim_z=4, max_obs=max_obs) - self.kf.F = np.array( - [ - [1, 0, 0, 0, 1, 0, 0], - [0, 1, 0, 0, 0, 1, 0], - [0, 0, 1, 0, 0, 0, 1], - [0, 0, 0, 1, 0, 0, 0], - [0, 0, 0, 0, 1, 0, 0], - [0, 0, 0, 0, 0, 1, 0], - [0, 0, 0, 0, 0, 0, 1], - ] - ) - self.kf.H = np.array( - [ - [1, 0, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0, 0], - [0, 0, 0, 1, 0, 0, 0], - ] - ) - - self.kf.R[2:, 2:] *= 10.0 - self.kf.P[ - 4:, 4: - ] *= 1000.0 # give high uncertainty to the unobservable initial velocities - self.kf.P *= 10.0 - - self.kf.Q[4:6, 4:6] *= self.Q_xy_scaling - self.kf.Q[-1, -1] *= self.Q_s_scaling - - self.kf.x[:4] = xyxy2xysr(bbox) - self.time_since_update = 0 - self.id = KalmanBoxTracker.count - KalmanBoxTracker.count += 1 - self.max_obs = max_obs - self.history = deque([], maxlen=self.max_obs) - self.hits = 0 - self.hit_streak = 0 - self.age = 0 - self.conf = bbox[-1] - self.cls = cls - """ - NOTE: [-1,-1,-1,-1,-1] is a compromising placeholder for non-observation status, the same for the return of - function k_previous_obs. It is ugly and I do not like it. But to support generate observation array in a - fast and unified way, which you would see below k_observations = np.array([k_previous_obs(...]]), - let's bear it for now. - """ - self.last_observation = np.array([-1, -1, -1, -1, -1]) # placeholder - self.observations = dict() - self.history_observations = deque([], maxlen=self.max_obs) - self.velocity = None - self.delta_t = delta_t - - def update(self, bbox, cls, det_ind): - """ - Updates the state vector with observed bbox. - """ - self.det_ind = det_ind - if bbox is not None: - self.conf = bbox[-1] - self.cls = cls - if self.last_observation.sum() >= 0: # no previous observation - previous_box = None - for i in range(self.delta_t): - dt = self.delta_t - i - if self.age - dt in self.observations: - previous_box = self.observations[self.age - dt] - break - if previous_box is None: - previous_box = self.last_observation - """ - Estimate the track speed direction with observations \Delta t steps away - """ - self.velocity = speed_direction(previous_box, bbox) - - """ - Insert new observations. This is a ugly way to maintain both self.observations - and self.history_observations. Bear it for the moment. - """ - self.last_observation = bbox - self.observations[self.age] = bbox - self.history_observations.append(bbox) - - self.time_since_update = 0 - self.hits += 1 - self.hit_streak += 1 - self.kf.update(xyxy2xysr(bbox)) - else: - self.kf.update(bbox) - - def predict(self): - """ - Advances the state vector and returns the predicted bounding box estimate. - """ - if (self.kf.x[6] + self.kf.x[2]) <= 0: - self.kf.x[6] *= 0.0 - - self.kf.predict() - self.age += 1 - if self.time_since_update > 0: - self.hit_streak = 0 - self.time_since_update += 1 - self.history.append(convert_x_to_bbox(self.kf.x)) - return self.history[-1] - - def get_state(self): - """ - Returns the current bounding box estimate. - """ - return convert_x_to_bbox(self.kf.x) - - -class KalmanBoxTrackerOBB(object): - """ - This class represents the internal state of individual tracked objects observed as oriented bbox. - """ - - count = 0 - - def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_a_scaling = 0.01): - """ - Initialises a tracker using initial bounding box. - - """ - # define constant velocity model - self.det_ind = det_ind - - self.Q_xy_scaling = Q_xy_scaling - self.Q_a_scaling = Q_a_scaling - - self.kf = KalmanFilterXYWHA(dim_x=10, dim_z=5, max_obs=max_obs) - self.kf.F = np.array( - [ - [1, 0, 0, 0, 0, 1, 0, 0, 0, 0], # cx = cx + vx - [0, 1, 0, 0, 0, 0, 1, 0, 0, 0], # cy = cy + vy - [0, 0, 1, 0, 0, 0, 0, 1, 0, 0], # w = w + vw - [0, 0, 0, 1, 0, 0, 0, 0, 1, 0], # h = h + vh - [0, 0, 0, 0, 1, 0, 0, 0, 0, 1], # a = a + va - [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 0, 1] - ] - ) - self.kf.H = np.array( - [ - [1, 0, 0, 0, 0, 0, 0, 0, 0 ,0], # cx - [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], # cy - [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], # w - [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], # h - [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], # angle - ] - ) - - self.kf.R[2:, 2:] *= 10.0 - self.kf.P[ - 5:, 5: - ] *= 1000.0 # give high uncertainty to the unobservable initial velocities - self.kf.P *= 10.0 - - self.kf.Q[5:7, 5:7] *= self.Q_xy_scaling - self.kf.Q[-1, -1] *= self.Q_a_scaling - - self.kf.x[:5] = bbox[:5].reshape((5, 1)) # x, y, w, h, angle (dont take confidence score) - self.time_since_update = 0 - self.id = KalmanBoxTrackerOBB.count - KalmanBoxTrackerOBB.count += 1 - self.max_obs = max_obs - self.history = deque([], maxlen=self.max_obs) - self.hits = 0 - self.hit_streak = 0 - self.age = 0 - self.conf = bbox[-1] - self.cls = cls - """ - NOTE: [-1,-1,-1,-1,-1] is a compromising placeholder for non-observation status, the same for the return of - function k_previous_obs. It is ugly and I do not like it. But to support generate observation array in a - fast and unified way, which you would see below k_observations = np.array([k_previous_obs(...]]), - let's bear it for now. - """ - self.last_observation = np.array([-1, -1, -1, -1, -1, -1]) #WARNING : -1 is a valid angle value - self.observations = dict() - self.history_observations = deque([], maxlen=self.max_obs) - self.velocity = None - self.delta_t = delta_t - - def update(self, bbox, cls, det_ind): - """ - Updates the state vector with observed bbox. - """ - self.det_ind = det_ind - if bbox is not None: - self.conf = bbox[-1] - self.cls = cls - if self.last_observation.sum() >= 0: # no previous observation - previous_box = None - for i in range(self.delta_t): - dt = self.delta_t - i - if self.age - dt in self.observations: - previous_box = self.observations[self.age - dt] - break - if previous_box is None: - previous_box = self.last_observation - """ - Estimate the track speed direction with observations \Delta t steps away - """ - self.velocity = speed_direction_obb(previous_box, bbox) - - """ - Insert new observations. This is a ugly way to maintain both self.observations - and self.history_observations. Bear it for the moment. - """ - self.last_observation = bbox - self.observations[self.age] = bbox - self.history_observations.append(bbox) - - self.time_since_update = 0 - self.hits += 1 - self.hit_streak += 1 - self.kf.update(bbox[:5].reshape((5, 1))) # x, y, w, h, angle as column vector (dont take confidence score) - else: - self.kf.update(bbox) - - def predict(self): - """ - Advances the state vector and returns the predicted bounding box estimate. - """ - if (self.kf.x[7] + self.kf.x[2]) <= 0: # Negative width - self.kf.x[7] *= 0.0 - if (self.kf.x[8] + self.kf.x[3]) <= 0: # Negative Height - self.kf.x[8] *= 0.0 - self.kf.predict() - self.age += 1 - if self.time_since_update > 0: - self.hit_streak = 0 - self.time_since_update += 1 - self.history.append(self.kf.x[0:5].reshape((1, 5))) - return self.history[-1] - - def get_state(self): - """ - Returns the current bounding box estimate. - """ - return self.kf.x[0:5].reshape((1, 5)) - - class OcSort(BaseTracker): """ OCSort Tracker: A tracking algorithm that utilizes motion-based tracking. @@ -356,9 +58,8 @@ def __init__( use_byte: bool = False, Q_xy_scaling: float = 0.01, Q_s_scaling: float = 0.0001, - is_obb: bool = False ): - super().__init__(max_age=max_age, per_class=per_class, asso_func=asso_func, is_obb=is_obb) + super().__init__(max_age=max_age, per_class=per_class, asso_func=asso_func) """ Sets key parameters for SORT """ @@ -373,9 +74,9 @@ def __init__( self.use_byte = use_byte self.Q_xy_scaling = Q_xy_scaling self.Q_s_scaling = Q_s_scaling - KalmanBoxTracker.count = 0 + KalmanBoxTrackerOBB.count = 0 - @BaseTracker.on_first_frame_setup + @BaseTracker.setup_decorator @BaseTracker.per_class_decorator def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> np.ndarray: """ @@ -535,139 +236,3 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> if len(ret) > 0: return np.concatenate(ret) return np.array([]) - - def plot_box_on_img(self, img: np.ndarray, box: tuple, conf: float, cls: int, id: int, thickness: int = 2, fontscale: float = 0.5) -> np.ndarray: - """ - Draws a bounding box with ID, confidence, and class information on an image. - - Parameters: - - img (np.ndarray): The image array to draw on. - - box (tuple): The bounding box coordinates as (x1, y1, x2, y2). - - conf (float): Confidence score of the detection. - - cls (int): Class ID of the detection. - - id (int): Unique identifier for the detection. - - thickness (int): The thickness of the bounding box. - - fontscale (float): The font scale for the text. - - Returns: - - np.ndarray: The image array with the bounding box drawn on it. - """ - if self.is_obb: - - angle = box[4] * 180.0 / np.pi # Convert radians to degrees - box_poly = ((box[0], box[1]), (box[2], box[3]), angle) - # print((width, height)) - rotrec = cv.boxPoints(box_poly) - box_poly = np.int_(rotrec) # Convert to integer - - # Draw the rectangle on the image - img = cv.polylines(img, [box_poly], isClosed=True, color=self.id_to_color(id), thickness=thickness) - - img = cv.putText( - img, - f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}, a: {box[4]:.2f}', - (int(box[0]), int(box[1]) - 10), - cv.FONT_HERSHEY_SIMPLEX, - fontscale, - self.id_to_color(id), - thickness - ) - else : - - img = cv.rectangle( - img, - (int(box[0]), int(box[1])), - (int(box[2]), int(box[3])), - self.id_to_color(id), - thickness - ) - img = cv.putText( - img, - f'id: {int(id)}, conf: {conf:.2f}, c: {int(cls)}', - (int(box[0]), int(box[1]) - 10), - cv.FONT_HERSHEY_SIMPLEX, - fontscale, - self.id_to_color(id), - thickness - ) - return img - - - def plot_trackers_trajectories(self, img: np.ndarray, observations: list, id: int) -> np.ndarray: - """ - Draws the trajectories of tracked objects based on historical observations. Each point - in the trajectory is represented by a circle, with the thickness increasing for more - recent observations to visualize the path of movement. - - Parameters: - - img (np.ndarray): The image array on which to draw the trajectories. - - observations (list): A list of bounding box coordinates representing the historical - observations of a tracked object. Each observation is in the format (x1, y1, x2, y2). - - id (int): The unique identifier of the tracked object for color consistency in visualization. - - Returns: - - np.ndarray: The image array with the trajectories drawn on it. - """ - for i, box in enumerate(observations): - trajectory_thickness = int(np.sqrt(float (i + 1)) * 1.2) - if self.is_obb: - img = cv.circle( - img, - (int(box[0]), int(box[1])), - 2, - color=self.id_to_color(int(id)), - thickness=trajectory_thickness - ) - else: - - img = cv.circle( - img, - (int((box[0] + box[2]) / 2), - int((box[1] + box[3]) / 2)), - 2, - color=self.id_to_color(int(id)), - thickness=trajectory_thickness - ) - return img - - def plot_results(self, img: np.ndarray, show_trajectories: bool, thickness: int = 2, fontscale: float = 0.5) -> np.ndarray: - """ - Visualizes the trajectories of all active tracks on the image. For each track, - it draws the latest bounding box and the path of movement if the history of - observations is longer than two. This helps in understanding the movement patterns - of each tracked object. - - Parameters: - - img (np.ndarray): The image array on which to draw the trajectories and bounding boxes. - - show_trajectories (bool): Whether to show the trajectories. - - thickness (int): The thickness of the bounding box. - - fontscale (float): The font scale for the text. - - Returns: - - np.ndarray: The image array with trajectories and bounding boxes of all active tracks. - """ - - # if values in dict - if self.per_class_active_tracks is not None: - for k in self.per_class_active_tracks.keys(): - active_tracks = self.per_class_active_tracks[k] - for a in active_tracks: - if a.history_observations: - if a.hits >= self.min_hits: - if len(a.history_observations) > 2: - box = a.history_observations[-1] - img = self.plot_box_on_img(img, box, a.conf, a.cls, a.id, thickness, fontscale) - if show_trajectories: - img = self.plot_trackers_trajectories(img, a.history_observations, a.id) - else: - for a in self.active_tracks: - if a.history_observations: - if a.hits >= self.min_hits: - if len(a.history_observations) > 2: - box = a.history_observations[-1] - img = self.plot_box_on_img(img, box, a.conf, a.cls, a.id, thickness, fontscale) - if show_trajectories: - img = self.plot_trackers_trajectories(img, a.history_observations, a.id) - - return img - \ No newline at end of file diff --git a/boxmot/trackers/strongsort/sort/track.py b/boxmot/trackers/strongsort/sort/track.py index 50d1e10983..c33367819c 100644 --- a/boxmot/trackers/strongsort/sort/track.py +++ b/boxmot/trackers/strongsort/sort/track.py @@ -3,7 +3,7 @@ import os import numpy as np -from boxmot.motion.kalman_filters.xyah_kf import KalmanFilterXYAH +from boxmot.motion.kalman_filters.aabb.xyah_kf import KalmanFilterXYAH class TrackState: diff --git a/boxmot/utils/selective_reid.py b/boxmot/utils/selective_reid.py new file mode 100644 index 0000000000..add2501b85 --- /dev/null +++ b/boxmot/utils/selective_reid.py @@ -0,0 +1,99 @@ + def aiou(self, bbox, candidates): + """ + IoU - Aspect Ratio + """ + candidates = np.array(candidates) + bbox_tl, bbox_br = bbox[:2], bbox[:2] + bbox[2:] + candidates_tl = candidates[:, :2] + candidates_br = candidates[:, :2] + candidates[:, 2:] + + tl = np.c_[np.maximum(bbox_tl[0], candidates_tl[:, 0])[:, np.newaxis], + np.maximum(bbox_tl[1], candidates_tl[:, 1])[:, np.newaxis]] + br = np.c_[np.minimum(bbox_br[0], candidates_br[:, 0])[:, np.newaxis], + np.minimum(bbox_br[1], candidates_br[:, 1])[:, np.newaxis]] + wh = np.maximum(0., br - tl) + + area_intersection = wh.prod(axis=1) + area_bbox = bbox[2:].prod() + area_candidates = candidates[:, 2:].prod(axis=1) + + iou = area_intersection / (area_bbox + area_candidates - area_intersection) + + aspect_ratio = bbox[2] / bbox[3] + candidates_aspect_ratio = candidates[:, 2] / candidates[:, 3] + arctan = np.arctan(aspect_ratio) - np.arctan(candidates_aspect_ratio) + v = 1 - ((4 / np.pi ** 2) * arctan ** 2) + alpha = v / (1 - iou + v) + + return iou, alpha + + def aiou_vectorized(self, bboxes1, bboxes2): + """ + Vectorized implementation of AIOU (IoU with aspect ratio consideration) + + Args: + bboxes1: numpy array of shape (N, 4) in format (x, y, w, h) + bboxes2: numpy array of shape (M, 4) in format (x, y, w, h) + + Returns: + ious: numpy array of shape (N, M) containing IoU values + alphas: numpy array of shape (N, M) containing alpha values + """ + # Convert (x, y, w, h) to (x1, y1, x2, y2) + bboxes1_x1y1 = bboxes1[:, :2] + bboxes1_x2y2 = bboxes1[:, :2] + bboxes1[:, 2:] + bboxes2_x1y1 = bboxes2[:, :2] + bboxes2_x2y2 = bboxes2[:, :2] + bboxes2[:, 2:] + + # Compute intersection + intersect_x1y1 = np.maximum(bboxes1_x1y1[:, None], bboxes2_x1y1[None, :]) + intersect_x2y2 = np.minimum(bboxes1_x2y2[:, None], bboxes2_x2y2[None, :]) + intersect_wh = np.maximum(0., intersect_x2y2 - intersect_x1y1) + + # Compute areas + intersect_area = intersect_wh.prod(axis=2) + bboxes1_area = bboxes1[:, 2:].prod(axis=1) + bboxes2_area = bboxes2[:, 2:].prod(axis=1) + union_area = bboxes1_area[:, None] + bboxes2_area[None, :] - intersect_area + + # Compute IoU + ious = intersect_area / union_area + + # Compute aspect ratios + bboxes1_aspect_ratio = bboxes1[:, 2] / bboxes1[:, 3] + bboxes2_aspect_ratio = bboxes2[:, 2] / bboxes2[:, 3] + + # Compute alpha + arctan_diff = np.arctan(bboxes1_aspect_ratio[:, None]) - np.arctan(bboxes2_aspect_ratio[None, :]) + v = 1 - ((4 / (np.pi ** 2)) * arctan_diff ** 2) + alphas = v / (1 - ious + v) + + return ious, alphas + + + def candidates_to_detections(self, tlwh: np.ndarray, confirmed_tracks): + tracklet_bboxes = np.array([trk.to_xywh() for trk in confirmed_tracks]) + + if len(tracklet_bboxes) == 0: + return list(range(tlwh.shape[0])), [] + + print(tlwh.shape) + print(tracklet_bboxes.shape) + + ious, alphas = self.aiou_vectorized(tlwh, tracklet_bboxes) + + matches = ious > 0.5 + match_counts = np.sum(matches, axis=1) + + risky_detections = np.where(match_counts != 1)[0].tolist() + safe_candidates = np.where(match_counts == 1)[0] + + safe_det_trk_pairs = [] + for i in safe_candidates: + candidate = np.argmax(ious[i]) + if alphas[i, candidate] > 0.6: + safe_det_trk_pairs.append((i, candidate)) + else: + risky_detections.append(i) + + return risky_detections, safe_det_trk_pairs \ No newline at end of file From 93010bc975ad2654fe1f60fae2e21e874e4b86ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 10:22:03 +0100 Subject: [PATCH 23/28] enable obb tracking --- boxmot/trackers/ocsort/ocsort.py | 165 +++++++++++++++++++++++++++++-- 1 file changed, 159 insertions(+), 6 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index f732b48dad..ddd977d2b4 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -6,20 +6,18 @@ import numpy as np from collections import deque -import cv2 as cv +from boxmot.motion.kalman_filters.aabb.xysr_kf import KalmanFilterXYSR from boxmot.utils.association import associate, linear_assignment from boxmot.trackers.basetracker import BaseTracker from boxmot.utils.ops import xyxy2xysr from boxmot.motion.kalman_filters.obb.xywha_kf import KalmanBoxTrackerOBB -def k_previous_obs(observations, cur_age, k, is_obb=False): + +def k_previous_obs(observations, cur_age, k): if len(observations) == 0: - if is_obb: - return [-1, -1, -1, -1, -1, -1] - else : - return [-1, -1, -1, -1, -1] + return [-1, -1, -1, -1, -1] for i in range(k): dt = k - i if cur_age - dt in observations: @@ -28,6 +26,161 @@ def k_previous_obs(observations, cur_age, k, is_obb=False): return observations[max_age] +def convert_x_to_bbox(x, score=None): + """ + Takes a bounding box in the centre form [x,y,s,r] and returns it in the form + [x1,y1,x2,y2] where x1,y1 is the top left and x2,y2 is the bottom right + """ + w = np.sqrt(x[2] * x[3]) + h = x[2] / w + if score is None: + return np.array( + [x[0] - w / 2.0, x[1] - h / 2.0, x[0] + w / 2.0, x[1] + h / 2.0] + ).reshape((1, 4)) + else: + return np.array( + [x[0] - w / 2.0, x[1] - h / 2.0, x[0] + w / 2.0, x[1] + h / 2.0, score] + ).reshape((1, 5)) + + +def speed_direction(bbox1, bbox2): + cx1, cy1 = (bbox1[0] + bbox1[2]) / 2.0, (bbox1[1] + bbox1[3]) / 2.0 + cx2, cy2 = (bbox2[0] + bbox2[2]) / 2.0, (bbox2[1] + bbox2[3]) / 2.0 + speed = np.array([cy2 - cy1, cx2 - cx1]) + norm = np.sqrt((cy2 - cy1) ** 2 + (cx2 - cx1) ** 2) + 1e-6 + return speed / norm + + +class KalmanBoxTracker(object): + """ + This class represents the internal state of individual tracked objects observed as bbox. + """ + + count = 0 + + def __init__(self, bbox, cls, det_ind, delta_t=3, max_obs=50, Q_xy_scaling = 0.01, Q_s_scaling = 0.0001): + """ + Initialises a tracker using initial bounding box. + + """ + # define constant velocity model + self.det_ind = det_ind + + self.Q_xy_scaling = Q_xy_scaling + self.Q_s_scaling = Q_s_scaling + + self.kf = KalmanFilterXYSR(dim_x=7, dim_z=4, max_obs=max_obs) + self.kf.F = np.array( + [ + [1, 0, 0, 0, 1, 0, 0], + [0, 1, 0, 0, 0, 1, 0], + [0, 0, 1, 0, 0, 0, 1], + [0, 0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 0, 1], + ] + ) + self.kf.H = np.array( + [ + [1, 0, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 1, 0, 0, 0], + ] + ) + + self.kf.R[2:, 2:] *= 10.0 + self.kf.P[ + 4:, 4: + ] *= 1000.0 # give high uncertainty to the unobservable initial velocities + self.kf.P *= 10.0 + + self.kf.Q[4:6, 4:6] *= self.Q_xy_scaling + self.kf.Q[-1, -1] *= self.Q_s_scaling + + self.kf.x[:4] = xyxy2xysr(bbox) + self.time_since_update = 0 + self.id = KalmanBoxTracker.count + KalmanBoxTracker.count += 1 + self.max_obs = max_obs + self.history = deque([], maxlen=self.max_obs) + self.hits = 0 + self.hit_streak = 0 + self.age = 0 + self.conf = bbox[-1] + self.cls = cls + """ + NOTE: [-1,-1,-1,-1,-1] is a compromising placeholder for non-observation status, the same for the return of + function k_previous_obs. It is ugly and I do not like it. But to support generate observation array in a + fast and unified way, which you would see below k_observations = np.array([k_previous_obs(...]]), + let's bear it for now. + """ + self.last_observation = np.array([-1, -1, -1, -1, -1]) # placeholder + self.observations = dict() + self.history_observations = deque([], maxlen=self.max_obs) + self.velocity = None + self.delta_t = delta_t + + def update(self, bbox, cls, det_ind): + """ + Updates the state vector with observed bbox. + """ + self.det_ind = det_ind + if bbox is not None: + self.conf = bbox[-1] + self.cls = cls + if self.last_observation.sum() >= 0: # no previous observation + previous_box = None + for i in range(self.delta_t): + dt = self.delta_t - i + if self.age - dt in self.observations: + previous_box = self.observations[self.age - dt] + break + if previous_box is None: + previous_box = self.last_observation + """ + Estimate the track speed direction with observations \Delta t steps away + """ + self.velocity = speed_direction(previous_box, bbox) + + """ + Insert new observations. This is a ugly way to maintain both self.observations + and self.history_observations. Bear it for the moment. + """ + self.last_observation = bbox + self.observations[self.age] = bbox + self.history_observations.append(bbox) + + self.time_since_update = 0 + self.hits += 1 + self.hit_streak += 1 + self.kf.update(xyxy2xysr(bbox)) + else: + self.kf.update(bbox) + + def predict(self): + """ + Advances the state vector and returns the predicted bounding box estimate. + """ + if (self.kf.x[6] + self.kf.x[2]) <= 0: + self.kf.x[6] *= 0.0 + + self.kf.predict() + self.age += 1 + if self.time_since_update > 0: + self.hit_streak = 0 + self.time_since_update += 1 + self.history.append(convert_x_to_bbox(self.kf.x)) + return self.history[-1] + + def get_state(self): + """ + Returns the current bounding box estimate. + """ + return convert_x_to_bbox(self.kf.x) + + class OcSort(BaseTracker): """ OCSort Tracker: A tracking algorithm that utilizes motion-based tracking. From 86a2826b7217a32a37f8fc4a155efc0e8f92d5a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 10:27:07 +0100 Subject: [PATCH 24/28] enable obb tracking --- boxmot/trackers/ocsort/ocsort.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/boxmot/trackers/ocsort/ocsort.py b/boxmot/trackers/ocsort/ocsort.py index ddd977d2b4..df9f84d251 100644 --- a/boxmot/trackers/ocsort/ocsort.py +++ b/boxmot/trackers/ocsort/ocsort.py @@ -14,10 +14,12 @@ from boxmot.motion.kalman_filters.obb.xywha_kf import KalmanBoxTrackerOBB - -def k_previous_obs(observations, cur_age, k): +def k_previous_obs(observations, cur_age, k, is_obb=False): if len(observations) == 0: - return [-1, -1, -1, -1, -1] + if is_obb: + return [-1, -1, -1, -1, -1, -1] + else : + return [-1, -1, -1, -1, -1] for i in range(k): dt = k - i if cur_age - dt in observations: @@ -227,7 +229,7 @@ def __init__( self.use_byte = use_byte self.Q_xy_scaling = Q_xy_scaling self.Q_s_scaling = Q_s_scaling - KalmanBoxTrackerOBB.count = 0 + KalmanBoxTracker.count = 0 @BaseTracker.setup_decorator @BaseTracker.per_class_decorator @@ -388,4 +390,4 @@ def update(self, dets: np.ndarray, img: np.ndarray, embs: np.ndarray = None) -> self.active_tracks.pop(i) if len(ret) > 0: return np.concatenate(ret) - return np.array([]) + return np.array([]) \ No newline at end of file From e6873d884304154cc3713999537bf52cda9feb15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 10:30:49 +0100 Subject: [PATCH 25/28] enable obb tracking --- boxmot/trackers/hybridsort/hybridsort.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boxmot/trackers/hybridsort/hybridsort.py b/boxmot/trackers/hybridsort/hybridsort.py index 144080e001..d375a93d37 100644 --- a/boxmot/trackers/hybridsort/hybridsort.py +++ b/boxmot/trackers/hybridsort/hybridsort.py @@ -128,7 +128,7 @@ def __init__( """ # define constant velocity model # if not orig and not args.kalman_GPR: - from boxmot.motion.kalman_filters.xysr_kf import KalmanFilterXYSR + from boxmot.motion.kalman_filters.aabb.xysr_kf import KalmanFilterXYSR self.kf = KalmanFilterXYSR(dim_x=9, dim_z=5, max_obs=max_obs) # u, v, s, c, r, ~u, ~v, ~s, ~c From 10c4bf16ea96e3496be2ebc2bf224a95546f34e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 12:14:26 +0100 Subject: [PATCH 26/28] fix --- boxmot/trackers/basetracker.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/boxmot/trackers/basetracker.py b/boxmot/trackers/basetracker.py index a8b8dbc096..88772e3350 100644 --- a/boxmot/trackers/basetracker.py +++ b/boxmot/trackers/basetracker.py @@ -111,12 +111,13 @@ def wrapper(self, *args, **kwargs): # Even if dets is empty (e.g., shape (0, 7)), this check will still pass if it's Nx7 if not self._first_dets_processed: dets = args[0] - if dets.ndim == 2 and dets.shape[1] == 6: - self.is_obb = False - self._first_dets_processed = True - elif dets.ndim == 2 and dets.shape[1] == 7: - self.is_obb = True - self._first_dets_processed = True + if dets is not None: + if dets.ndim == 2 and dets.shape[1] == 6: + self.is_obb = False + self._first_dets_processed = True + elif dets.ndim == 2 and dets.shape[1] == 7: + self.is_obb = True + self._first_dets_processed = True if not self._first_frame_processed: img = args[1] @@ -191,12 +192,10 @@ def check_inputs(self, dets, img): assert ( len(dets.shape) == 2 ), "Unsupported 'dets' dimensions, valid number of dimensions is two" - if self.is_obb : - + if self.is_obb: assert ( dets.shape[1] == 7 ), "Unsupported 'dets' 2nd dimension lenght, valid lenghts is 6 (cx,cy,w,h,angle,conf,cls)" - else : assert ( dets.shape[1] == 6 From c9fea47036f2b0526c83125546f94a8992988c97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 12:18:09 +0100 Subject: [PATCH 27/28] fix --- boxmot/utils/selective_reid.py | 99 ---------------------------------- 1 file changed, 99 deletions(-) delete mode 100644 boxmot/utils/selective_reid.py diff --git a/boxmot/utils/selective_reid.py b/boxmot/utils/selective_reid.py deleted file mode 100644 index add2501b85..0000000000 --- a/boxmot/utils/selective_reid.py +++ /dev/null @@ -1,99 +0,0 @@ - def aiou(self, bbox, candidates): - """ - IoU - Aspect Ratio - """ - candidates = np.array(candidates) - bbox_tl, bbox_br = bbox[:2], bbox[:2] + bbox[2:] - candidates_tl = candidates[:, :2] - candidates_br = candidates[:, :2] + candidates[:, 2:] - - tl = np.c_[np.maximum(bbox_tl[0], candidates_tl[:, 0])[:, np.newaxis], - np.maximum(bbox_tl[1], candidates_tl[:, 1])[:, np.newaxis]] - br = np.c_[np.minimum(bbox_br[0], candidates_br[:, 0])[:, np.newaxis], - np.minimum(bbox_br[1], candidates_br[:, 1])[:, np.newaxis]] - wh = np.maximum(0., br - tl) - - area_intersection = wh.prod(axis=1) - area_bbox = bbox[2:].prod() - area_candidates = candidates[:, 2:].prod(axis=1) - - iou = area_intersection / (area_bbox + area_candidates - area_intersection) - - aspect_ratio = bbox[2] / bbox[3] - candidates_aspect_ratio = candidates[:, 2] / candidates[:, 3] - arctan = np.arctan(aspect_ratio) - np.arctan(candidates_aspect_ratio) - v = 1 - ((4 / np.pi ** 2) * arctan ** 2) - alpha = v / (1 - iou + v) - - return iou, alpha - - def aiou_vectorized(self, bboxes1, bboxes2): - """ - Vectorized implementation of AIOU (IoU with aspect ratio consideration) - - Args: - bboxes1: numpy array of shape (N, 4) in format (x, y, w, h) - bboxes2: numpy array of shape (M, 4) in format (x, y, w, h) - - Returns: - ious: numpy array of shape (N, M) containing IoU values - alphas: numpy array of shape (N, M) containing alpha values - """ - # Convert (x, y, w, h) to (x1, y1, x2, y2) - bboxes1_x1y1 = bboxes1[:, :2] - bboxes1_x2y2 = bboxes1[:, :2] + bboxes1[:, 2:] - bboxes2_x1y1 = bboxes2[:, :2] - bboxes2_x2y2 = bboxes2[:, :2] + bboxes2[:, 2:] - - # Compute intersection - intersect_x1y1 = np.maximum(bboxes1_x1y1[:, None], bboxes2_x1y1[None, :]) - intersect_x2y2 = np.minimum(bboxes1_x2y2[:, None], bboxes2_x2y2[None, :]) - intersect_wh = np.maximum(0., intersect_x2y2 - intersect_x1y1) - - # Compute areas - intersect_area = intersect_wh.prod(axis=2) - bboxes1_area = bboxes1[:, 2:].prod(axis=1) - bboxes2_area = bboxes2[:, 2:].prod(axis=1) - union_area = bboxes1_area[:, None] + bboxes2_area[None, :] - intersect_area - - # Compute IoU - ious = intersect_area / union_area - - # Compute aspect ratios - bboxes1_aspect_ratio = bboxes1[:, 2] / bboxes1[:, 3] - bboxes2_aspect_ratio = bboxes2[:, 2] / bboxes2[:, 3] - - # Compute alpha - arctan_diff = np.arctan(bboxes1_aspect_ratio[:, None]) - np.arctan(bboxes2_aspect_ratio[None, :]) - v = 1 - ((4 / (np.pi ** 2)) * arctan_diff ** 2) - alphas = v / (1 - ious + v) - - return ious, alphas - - - def candidates_to_detections(self, tlwh: np.ndarray, confirmed_tracks): - tracklet_bboxes = np.array([trk.to_xywh() for trk in confirmed_tracks]) - - if len(tracklet_bboxes) == 0: - return list(range(tlwh.shape[0])), [] - - print(tlwh.shape) - print(tracklet_bboxes.shape) - - ious, alphas = self.aiou_vectorized(tlwh, tracklet_bboxes) - - matches = ious > 0.5 - match_counts = np.sum(matches, axis=1) - - risky_detections = np.where(match_counts != 1)[0].tolist() - safe_candidates = np.where(match_counts == 1)[0] - - safe_det_trk_pairs = [] - for i in safe_candidates: - candidate = np.argmax(ious[i]) - if alphas[i, candidate] > 0.6: - safe_det_trk_pairs.append((i, candidate)) - else: - risky_detections.append(i) - - return risky_detections, safe_det_trk_pairs \ No newline at end of file From 2d36c6a095a66362f7b20086f9ca6d4c6b914a57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mikel=20Brostr=C3=B6m?= Date: Fri, 31 Jan 2025 12:38:27 +0100 Subject: [PATCH 28/28] add obb example --- examples/det/obb.ipynb | 118 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 118 insertions(+) create mode 100644 examples/det/obb.ipynb diff --git a/examples/det/obb.ipynb b/examples/det/obb.ipynb new file mode 100644 index 0000000000..749c2a6660 --- /dev/null +++ b/examples/det/obb.ipynb @@ -0,0 +1,118 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "2\r" + ] + } + ], + "source": [ + "import cv2\n", + "import numpy as np\n", + "from pathlib import Path\n", + "\n", + "from boxmot import OcSort\n", + "\n", + "\n", + "# Initialize the tracker\n", + "tracker = OcSort(\n", + " asso_func=\"centroid\",\n", + " min_hits=10,\n", + " asso_threshold=0.98,\n", + " det_thresh = 0.7,\n", + " max_age=20,\n", + " use_byte=True,\n", + " Q_xy_scaling = 0.01,\n", + " Q_s_scaling = 0.0001,\n", + ")\n", + "\n", + "def get_parabolic_array(i):\n", + " \"\"\"\n", + " Generates coordinates where x increases linearly,\n", + " y follows a parabolic curve (y = base + coeff * i^2),\n", + " and the angle matches the tangent of the curve.\n", + " \"\"\"\n", + " # -- FIRST ROW --\n", + " # Define x1, y1\n", + " x1 = 144 + i\n", + " y1 = 212 + 0.01 * (i ** 2)\n", + " \n", + " # Slope = 2 * coeff * i => for 0.01 * i^2, slope = 2*0.01*i = 0.02*i\n", + " slope1 = 0.02 * i\n", + " # Angle in radians\n", + " angle1 = np.arctan(slope1)\n", + " \n", + " # -- SECOND ROW --\n", + " # Define x2, y2 with a different coefficient\n", + " x2 = 425 + i\n", + " y2 = 281 + 0.02 * (i ** 2)\n", + " \n", + " # Slope for 0.02 * i^2 is 2*0.02*i = 0.04*i\n", + " slope2 = 0.04 * i\n", + " # Angle in radians\n", + " angle2 = np.arctan(slope2)\n", + " \n", + " # Build the array\n", + " det = np.array([\n", + " [x1, y1, 45, 30, angle1, 0.82, 0], # row 1\n", + " [x2, y2, 45, 30, angle2, 0.72, 65] # row 2\n", + " ])\n", + " \n", + " return det\n", + "\n", + "\n", + "for i in range(0, 100):\n", + "\n", + " #frame = cv2.imread(str(img_dir / (file.stem + '.png')))\n", + " frame = np.zeros((1080,1080,3))\n", + " \n", + " det = get_parabolic_array(i)\n", + "\n", + " # Update the tracker\n", + " res = tracker.update(det, frame) # --> M X (x, y, x, y, id, conf, cls, ind)\n", + " \n", + " # Plot tracking results on the image\n", + " tracker.plot_results(frame, show_trajectories=True, fontscale=2, thickness=4)\n", + "\n", + " # Display the frame\n", + " cv2.imshow('BoXMOT', frame)\n", + "\n", + " print(len(tracker.active_tracks),end='\\r')\n", + "\n", + " key = cv2.waitKey(1) & 0xFF\n", + " if key == ord('q') or key ==27:\n", + " break\n", + "\n", + "cv2.destroyAllWindows()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "boxmot-YDNZdsaB-py3.11", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.5" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}