update

tracking/scripts/AB3DMOT_libs/kalman_filter.py

 # Author: Xinshuo Weng
 # email: xinshuo.weng@gmail.com
-
+import math
 import numpy as np
 from filterpy.kalman import KalmanFilter
+from sklearn.decomposition import PCA
+
+
+NUM_FRAME = 5
+DIST_THRED = 0.5
+
+def center_rot_y_f(points):
+
+    x_list = points[:, 0]
+    y_list = points[:, 1]
+
+    pca = PCA(n_components=2)
+    pca.fit(points)
+    PCA(copy=True, n_components=2, whiten=False)
+
+    orientation = pca.components_[0]
+
+    dx = x_list[-1] - x_list[0]
+    dy = y_list[-1] - y_list[0]
+    alpha = np.dot(np.array([dx, dy]), orientation)
+
+    if alpha < 0:
+        orientation = -orientation
+
+    center_rot_y = math.atan2(orientation[1], orientation[0])
+
+    return center_rot_y
 
 
 class KalmanBoxTracker(object):
@@ -78,11 +105,36 @@ class KalmanBoxTracker(object):
         self.still_first = True
         self.age = 0
         self.info = info  # other info associated
+        self.type_history = [info[1]]
+        self.color_history = [info[-1]]
+        self.license_plate_number_history = [info[-2]]
+        self.name_history = [info[-3]]
+        last_x_y = [self.kf.x[0][0], self.kf.x[1][0]]
+        self.last_x_y_list = [last_x_y]
 
     def update(self, bbox3D, info):
         """
         Updates the state vector with observed bbox.
         """
+        points = self.last_x_y_list[:]
+        points = np.array(points)
+        #points = np.squeeze(points, axis=(2,))
+        x_list = points[:, 0]
+        y_list = points[:, 1]
+        if len(points) >= NUM_FRAME and abs(x_list[-1] - x_list[0]) > DIST_THRED or abs(y_list[-1] - y_list[0]) > DIST_THRED:
+            center_rot_y = center_rot_y_f(points)
+
+            rot_y = bbox3D[3]
+            if rot_y > np.pi: rot_y -= int((rot_y + np.pi)/(2*np.pi)) * np.pi * 2
+            if rot_y < -np.pi: rot_y += int((np.pi - rot_y)/(2*np.pi)) * np.pi * 2
+
+            if abs(center_rot_y - rot_y) > np.pi / 2.0 and abs(
+                    center_rot_y - rot_y) < np.pi * 3 / 2.0:  # if the angle of two theta is not acute angle
+                rot_y += np.pi
+                if rot_y > np.pi: rot_y -= np.pi * 2  # make the theta still in the range
+                if rot_y < -np.pi: rot_y += np.pi * 2
+            bbox3D[3] = rot_y
+
         self.time_since_update = 0
         self.history = []
         self.hits += 1
@@ -125,6 +177,12 @@ class KalmanBoxTracker(object):
         """
         Advances the state vector and returns the predicted bounding box estimate.
         """
+        if self.time_since_update == 0:
+            last_x_y = [self.kf.x[0][0], self.kf.x[1][0]]
+            self.last_x_y_list.append(last_x_y)
+            if len(self.last_x_y_list) > NUM_FRAME:
+                del(self.last_x_y_list[0])
+
         self.kf.predict()
         if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2
         if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2

tracking/scripts/AB3DMOT_libs/model.py

@@ -5,7 +5,7 @@ import numpy as np
 # from sklearn.utils.linear_assignment_ import linear_assignment    # deprecated
 from scipy.optimize import linear_sum_assignment
 from AB3DMOT_libs.bbox_utils import convert_3dbox_to_8corner, iou3d
-from AB3DMOT_libs.kalman_filter import KalmanBoxTracker
+from AB3DMOT_libs.kalman_filter import KalmanBoxTracker, NUM_FRAME
 
 
 def associate_detections_to_trackers(detections, trackers, iou_threshold=0.01):
@@ -113,12 +113,13 @@ class AB3DMOT(object):  # A baseline of 3D multi-object tracking
                 d = matched[np.where(matched[:, 1] == t)[0], 0]  # a list of index
                 ori_info = trk.info
                 cur_info = info[d, :][0]
-                if ori_info[-1] !='None':
-                    cur_info[-1] = ori_info[-1]
-                if ori_info[-2] !='None':
-                    cur_info[-2] = ori_info[-2]
-                if ori_info[-3] !='None':
-                    cur_info[-3] = ori_info[-3]
+                if cur_info[1] == ori_info[1]:
+                    if ori_info[-1] !='None':
+                        cur_info[-1] = ori_info[-1]
+                    if ori_info[-2] !='None':
+                        cur_info[-2] = ori_info[-2]
+                    if ori_info[-3] !='None':
+                        cur_info[-3] = ori_info[-3]
                 trk.update(dets[d, :][0], cur_info)
             else:
                 trk.info[0] = info[0][0]

tracking/scripts/AB3DMOT_libs/model_2.py

+# Author: Xinshuo Weng
+# email: xinshuo.weng@gmail.com
+
+import numpy as np
+# from sklearn.utils.linear_assignment_ import linear_assignment    # deprecated
+from scipy.optimize import linear_sum_assignment
+from AB3DMOT_libs.bbox_utils import convert_3dbox_to_8corner, iou3d
+from AB3DMOT_libs.kalman_filter import KalmanBoxTracker
+
+
+TYPE_THRED = 10
+COLOR_THRED = 10
+LICENSE_PLATE_NUMBER_THRED = 10
+NAME_THRED = 10
+
+
+def associate_detections_to_trackers(detections, trackers, iou_threshold=0.01):
+    """
+    Assigns detections to tracked object (both represented as bounding boxes)
+
+    detections:  N x 8 x 3
+    trackers:    M x 8 x 3
+
+
+    Returns 3 lists of matches, unmatched_detections and unmatched_trackers
+    """
+    if (len(trackers) == 0):
+        return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 8, 3), dtype=int)
+    iou_matrix = np.zeros((len(detections), len(trackers)), dtype=np.float32)
+
+    for d, det in enumerate(detections):
+        for t, trk in enumerate(trackers):
+            iou_matrix[d, t] = iou3d(det, trk)[0]  # det: 8 x 3, trk: 8 x 3
+    # matched_indices = linear_assignment(-iou_matrix)
+    # hougarian algorithm, compatible to linear_assignment in sklearn.utils
+
+    row_ind, col_ind = linear_sum_assignment(-iou_matrix)  # hougarian algorithm
+    matched_indices = np.stack((row_ind, col_ind), axis=1)
+
+    unmatched_detections = []
+    for d, det in enumerate(detections):
+        if (d not in matched_indices[:, 0]): unmatched_detections.append(d)
+    unmatched_trackers = []
+    for t, trk in enumerate(trackers):
+        if (t not in matched_indices[:, 1]): unmatched_trackers.append(t)
+
+    # filter out matched with low IOU
+    matches = []
+    for m in matched_indices:
+        if (iou_matrix[m[0], m[1]] < iou_threshold):
+            unmatched_detections.append(m[0])
+            unmatched_trackers.append(m[1])
+        else:
+            matches.append(m.reshape(1, 2))
+    if (len(matches) == 0):
+        matches = np.empty((0, 2), dtype=int)
+    else:
+        matches = np.concatenate(matches, axis=0)
+
+    return matches, np.array(unmatched_detections), np.array(unmatched_trackers)
+
+
+class AB3DMOT(object):  # A baseline of 3D multi-object tracking
+    # max age will preserve the bbox does not appear no more than 2 frames, interpolate the detection
+    def __init__(self, max_age=2,
+                 min_hits=3):
+        """
+        Sets key parameters for SORT
+        """
+        self.max_age = max_age
+        self.min_hits = min_hits
+        self.trackers = []
+        self.frame_count = 0
+        self.reorder = [3, 4, 5, 6, 2, 1, 0]
+        self.reorder_back = [6, 5, 4, 0, 1, 2, 3]
+
+    def update(self, dets_all):
+        """
+        Params:
+          dets_all: dict
+            dets - a numpy array of detections in the format [[h,w,l,x,y,z,theta],...]
+            info: a array of other info for each det
+        Requires: this method must be called once for each frame even with empty detections.
+        Returns the a similar array, where the last column is the object ID.
+
+        NOTE: The number of objects returned may differ from the number of detections provided.
+        """
+        dets, info = dets_all['dets'], dets_all['info']  # dets: N x 7, float numpy array
+
+        # reorder the data to put x,y,z in front to be compatible with the state transition matrix
+        # where the constant velocity model is defined in the first three rows of the matrix
+        dets = dets[:, self.reorder]  # reorder the data to [[x,y,z,theta,l,w,h], ...]
+        self.frame_count += 1
+
+        trks = np.zeros((len(self.trackers), 7))  # N x 7 , # get predicted locations from existing trackers.
+        to_del = []
+        ret = []
+        for t, trk in enumerate(trks):
+            pos = self.trackers[t].predict().reshape((-1, 1))
+            trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]]
+            if (np.any(np.isnan(pos))):
+                to_del.append(t)
+        trks = np.ma.compress_rows(np.ma.masked_invalid(trks))
+        for t in reversed(to_del):
+            self.trackers.pop(t)
+
+        dets_8corner = [convert_3dbox_to_8corner(det_tmp) for det_tmp in dets]
+        if len(dets_8corner) > 0:
+            dets_8corner = np.stack(dets_8corner, axis=0)
+        else:
+            dets_8corner = []
+        trks_8corner = [convert_3dbox_to_8corner(trk_tmp) for trk_tmp in trks]
+        if len(trks_8corner) > 0: trks_8corner = np.stack(trks_8corner, axis=0)
+        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets_8corner, trks_8corner)
+
+        # update matched trackers with assigned detections
+        for t, trk in enumerate(self.trackers):
+            if t not in unmatched_trks:
+                d = matched[np.where(matched[:, 1] == t)[0], 0]  # a list of index
+                ori_info = trk.info
+                cur_info = info[d, :][0]
+
+                max_type = cur_info[1]
+                if len(trk.type_history) >= TYPE_THRED:
+                    max_type = max(trk.type_history, key=trk.type_history.count)
+                    del trk.type_history[0]
+                trk.type_history.append(cur_info[1])
+                cur_info[1] = max_type
+
+                max_color = cur_info[-1]
+                if len(trk.color_history) >= COLOR_THRED:
+                    max_color = max(trk.color_history, key=trk.color_history.count)
+                    del trk.color_history[0]
+                if cur_info[-1] != 'None':
+                    trk.color_history.append(cur_info[-1])
+                cur_info[-1] = max_color
+
+                max_license_plate_number_history = cur_info[-2]
+                if len(trk.license_plate_number_history) >= LICENSE_PLATE_NUMBER_THRED:
+                    max_license_plate_number_history = max(trk.license_plate_number_history, key=trk.license_plate_number_history.count)
+                    del trk.license_plate_number_history[0]
+                if cur_info[-2] != 'None':
+                    trk.license_plate_number_history.append(cur_info[-2])
+                cur_info[-2] = max_license_plate_number_history
+
+                max_name_history = cur_info[-3]
+                if len(trk.name_history) >= NAME_THRED:
+                    max_name_history = max(trk.name_history, key=trk.name_history.count)
+                    del trk.name_history[0]
+                if cur_info[-3] != 'None':
+                    trk.name_history.append(cur_info[-3])
+                cur_info[-3] = max_name_history
+
+                #if cur_info[1] == ori_info[1]:
+                #    if ori_info[-1] !='None':
+                #        cur_info[-1] = ori_info[-1]
+                #    if ori_info[-2] !='None':
+                #        cur_info[-2] = ori_info[-2]
+                #    if ori_info[-3] !='None':
+                #        cur_info[-3] = ori_info[-3]
+                trk.update(dets[d, :][0], cur_info)
+            else:
+                trk.info[0] = info[0][0]
+
+        # create and initialise new trackers for unmatched detections
+        for i in unmatched_dets:  # a scalar of index
+            trk = KalmanBoxTracker(dets[i, :], info[i, :])
+            self.trackers.append(trk)
+        i = len(self.trackers)
+        for trk in reversed(self.trackers):
+            d = trk.get_state()  # bbox location
+            d = d[self.reorder_back]  # change format from [x,y,z,theta,l,w,h] to [h,w,l,x,y,z,theta]
+
+            orientation = trk.get_orientation()
+
+            if ((trk.time_since_update < self.max_age) and (
+                    trk.hits >= self.min_hits or self.frame_count <= self.min_hits)):
+            # if trk.time_since_update < self.max_age:
+                ret.append(
+                    np.concatenate((d, [trk.id + 1], orientation, trk.info)).reshape(1, -1))  # +1 as MOT benchmark requires positive
+            i -= 1
+
+            # remove dead tracklet
+            if (trk.time_since_update >= self.max_age):
+                self.trackers.pop(i)
+        if (len(ret) > 0): return np.concatenate(ret)  # h,w,l,x,y,z,theta, ID, other info, confidence
+        return np.empty((0, 17))
+
+
+    def predict(self, timestamp):
+
+        rets = []
+
+        if len(self.trackers) == 0:
+            return rets
+        
+        pre_timestamp = int(self.trackers[0].info[0])
+        if (timestamp - pre_timestamp) > 450:
+            return rets
+
+        count = int(round((timestamp - pre_timestamp) / 100.0))
+        index = 1
+        while index < count:
+            self.frame_count += 1
+
+            trks = np.zeros((len(self.trackers), 7))
+            to_del = []
+            ret = []
+            for t, trk in enumerate(trks):
+                pos = self.trackers[t].predict().reshape((-1, 1))
+                trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]]
+                if (np.any(np.isnan(pos))):
+                    to_del.append(t)
+            for t in reversed(to_del):
+                self.trackers.pop(t)
+
+            for trk in reversed(self.trackers):
+                d = trk.get_state()  # bbox location
+                d = d[self.reorder_back]  # change format from [x,y,z,theta,l,w,h] to [h,w,l,x,y,z,theta]
+
+                orientation = trk.get_orientation()
+
+                trk.info[0] = pre_timestamp + 100*index
+                ret.append(
+                    np.concatenate((d, [trk.id + 1], orientation, trk.info)).reshape(1, -1))  # +1 as MOT benchmark requires positive
+
+            rets.append(np.concatenate(ret))
+            index += 1
+        return rets

tracking/scripts/AB3DMOT_libs/model_3.py

+# Author: Xinshuo Weng
+# email: xinshuo.weng@gmail.com
+
+import numpy as np
+# from sklearn.utils.linear_assignment_ import linear_assignment    # deprecated
+from scipy.optimize import linear_sum_assignment
+from AB3DMOT_libs.bbox_utils import convert_3dbox_to_8corner, iou3d
+from AB3DMOT_libs.kalman_filter import KalmanBoxTracker
+
+
+TYPE_THRED = 10
+COLOR_THRED = 10
+LICENSE_PLATE_NUMBER_THRED = 10
+NAME_THRED = 10
+
+
+def associate_detections_to_trackers(detections, trackers, iou_threshold=0.01):
+    """
+    Assigns detections to tracked object (both represented as bounding boxes)
+
+    detections:  N x 8 x 3
+    trackers:    M x 8 x 3
+
+
+    Returns 3 lists of matches, unmatched_detections and unmatched_trackers
+    """
+    if (len(trackers) == 0):
+        return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 8, 3), dtype=int)
+    iou_matrix = np.zeros((len(detections), len(trackers)), dtype=np.float32)
+
+    for d, det in enumerate(detections):
+        for t, trk in enumerate(trackers):
+            iou_matrix[d, t] = iou3d(det, trk)[0]  # det: 8 x 3, trk: 8 x 3
+    # matched_indices = linear_assignment(-iou_matrix)
+    # hougarian algorithm, compatible to linear_assignment in sklearn.utils
+
+    row_ind, col_ind = linear_sum_assignment(-iou_matrix)  # hougarian algorithm
+    matched_indices = np.stack((row_ind, col_ind), axis=1)
+
+    unmatched_detections = []
+    for d, det in enumerate(detections):
+        if (d not in matched_indices[:, 0]): unmatched_detections.append(d)
+    unmatched_trackers = []
+    for t, trk in enumerate(trackers):
+        if (t not in matched_indices[:, 1]): unmatched_trackers.append(t)
+
+    # filter out matched with low IOU
+    matches = []
+    for m in matched_indices:
+        if (iou_matrix[m[0], m[1]] < iou_threshold):
+            unmatched_detections.append(m[0])
+            unmatched_trackers.append(m[1])
+        else:
+            matches.append(m.reshape(1, 2))
+    if (len(matches) == 0):
+        matches = np.empty((0, 2), dtype=int)
+    else:
+        matches = np.concatenate(matches, axis=0)
+
+    return matches, np.array(unmatched_detections), np.array(unmatched_trackers)
+
+
+class AB3DMOT(object):  # A baseline of 3D multi-object tracking
+    # max age will preserve the bbox does not appear no more than 2 frames, interpolate the detection
+    def __init__(self, max_age=2,
+                 min_hits=3):
+        """
+        Sets key parameters for SORT
+        """
+        self.max_age = max_age
+        self.min_hits = min_hits
+        self.trackers = []
+        self.frame_count = 0
+        self.reorder = [3, 4, 5, 6, 2, 1, 0]
+        self.reorder_back = [6, 5, 4, 0, 1, 2, 3]
+
+    def update(self, dets_all):
+        """
+        Params:
+          dets_all: dict
+            dets - a numpy array of detections in the format [[h,w,l,x,y,z,theta],...]
+            info: a array of other info for each det
+        Requires: this method must be called once for each frame even with empty detections.
+        Returns the a similar array, where the last column is the object ID.
+
+        NOTE: The number of objects returned may differ from the number of detections provided.
+        """
+        dets, info = dets_all['dets'], dets_all['info']  # dets: N x 7, float numpy array
+
+        # reorder the data to put x,y,z in front to be compatible with the state transition matrix
+        # where the constant velocity model is defined in the first three rows of the matrix
+        dets = dets[:, self.reorder]  # reorder the data to [[x,y,z,theta,l,w,h], ...]
+        self.frame_count += 1
+
+        trks = np.zeros((len(self.trackers), 7))  # N x 7 , # get predicted locations from existing trackers.
+        to_del = []
+        ret = []
+        for t, trk in enumerate(trks):
+            pos = self.trackers[t].predict().reshape((-1, 1))
+            trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]]
+            if (np.any(np.isnan(pos))):
+                to_del.append(t)
+        trks = np.ma.compress_rows(np.ma.masked_invalid(trks))
+        for t in reversed(to_del):
+            self.trackers.pop(t)
+
+        dets_8corner = [convert_3dbox_to_8corner(det_tmp) for det_tmp in dets]
+        if len(dets_8corner) > 0:
+            dets_8corner = np.stack(dets_8corner, axis=0)
+        else:
+            dets_8corner = []
+        trks_8corner = [convert_3dbox_to_8corner(trk_tmp) for trk_tmp in trks]
+        if len(trks_8corner) > 0: trks_8corner = np.stack(trks_8corner, axis=0)
+        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets_8corner, trks_8corner)
+
+        # update matched trackers with assigned detections
+        for t, trk in enumerate(self.trackers):
+            if t not in unmatched_trks:
+                d = matched[np.where(matched[:, 1] == t)[0], 0]  # a list of index
+                ori_info = trk.info
+                cur_info = info[d, :][0]
+
+                if len(trk.type_history) < TYPE_THRED:
+                    trk.type_history.append(cur_info[1])
+                else:
+                    max_type = max(trk.type_history, key=trk.type_history.count)
+                    cur_info[1] = max_type
+
+                if len(trk.color_history) < COLOR_THRED:
+                    if cur_info[-1] != 'None':
+                        trk.color_history.append(cur_info[-1])
+                else:
+                    max_color = max(trk.color_history, key=trk.color_history.count)
+                    cur_info[-1] = max_color
+
+                if len(trk.license_plate_number_history) < LICENSE_PLATE_NUMBER_THRED:
+                    if cur_info[-2] != 'None':
+                        trk.license_plate_number_history.append(cur_info[-2])
+                else:
+                    max_license_plate_number_history = max(trk.license_plate_number_history, key=trk.license_plate_number_history.count)
+                    cur_info[-2] = max_license_plate_number_history
+
+                if len(trk.name_history) < NAME_THRED:
+                    if cur_info[-3] != 'None':
+                        trk.name_history.append(cur_info[-3])
+                else:
+                    max_name_history = max(trk.name_history, key=trk.name_history.count)
+                    cur_info[-3] = max_name_history
+
+                #if cur_info[1] == ori_info[1]:
+                #    if ori_info[-1] !='None':
+                #        cur_info[-1] = ori_info[-1]
+                #    if ori_info[-2] !='None':
+                #        cur_info[-2] = ori_info[-2]
+                #    if ori_info[-3] !='None':
+                #        cur_info[-3] = ori_info[-3]
+                trk.update(dets[d, :][0], cur_info)
+            else:
+                trk.info[0] = info[0][0]
+
+        # create and initialise new trackers for unmatched detections
+        for i in unmatched_dets:  # a scalar of index
+            trk = KalmanBoxTracker(dets[i, :], info[i, :])
+            self.trackers.append(trk)
+        i = len(self.trackers)
+        for trk in reversed(self.trackers):
+            d = trk.get_state()  # bbox location
+            d = d[self.reorder_back]  # change format from [x,y,z,theta,l,w,h] to [h,w,l,x,y,z,theta]
+
+            orientation = trk.get_orientation()
+
+            if ((trk.time_since_update < self.max_age) and (
+                    trk.hits >= self.min_hits or self.frame_count <= self.min_hits)):
+            # if trk.time_since_update < self.max_age:
+                ret.append(
+                    np.concatenate((d, [trk.id + 1], orientation, trk.info)).reshape(1, -1))  # +1 as MOT benchmark requires positive
+            i -= 1
+
+            # remove dead tracklet
+            if (trk.time_since_update >= self.max_age):
+                self.trackers.pop(i)
+        if (len(ret) > 0): return np.concatenate(ret)  # h,w,l,x,y,z,theta, ID, other info, confidence
+        return np.empty((0, 17))
+
+
+    def predict(self, timestamp):
+
+        rets = []
+
+        if len(self.trackers) == 0:
+            return rets
+        
+        pre_timestamp = int(self.trackers[0].info[0])
+        if (timestamp - pre_timestamp) > 450:
+            return rets
+
+        count = int(round((timestamp - pre_timestamp) / 100.0))
+        index = 1
+        while index < count:
+            self.frame_count += 1
+
+            trks = np.zeros((len(self.trackers), 7))
+            to_del = []
+            ret = []
+            for t, trk in enumerate(trks):
+                pos = self.trackers[t].predict().reshape((-1, 1))
+                trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]]
+                if (np.any(np.isnan(pos))):
+                    to_del.append(t)
+            for t in reversed(to_del):
+                self.trackers.pop(t)
+
+            for trk in reversed(self.trackers):
+                d = trk.get_state()  # bbox location
+                d = d[self.reorder_back]  # change format from [x,y,z,theta,l,w,h] to [h,w,l,x,y,z,theta]
+
+                orientation = trk.get_orientation()
+
+                trk.info[0] = pre_timestamp + 100*index
+                ret.append(
+                    np.concatenate((d, [trk.id + 1], orientation, trk.info)).reshape(1, -1))  # +1 as MOT benchmark requires positive
+
+            rets.append(np.concatenate(ret))
+            index += 1
+        return rets

tracking/scripts/AB3DMOT_libs/model_4.py

+# Author: Xinshuo Weng
+# email: xinshuo.weng@gmail.com
+
+import numpy as np
+# from sklearn.utils.linear_assignment_ import linear_assignment    # deprecated
+from scipy.optimize import linear_sum_assignment
+from AB3DMOT_libs.bbox_utils import convert_3dbox_to_8corner, iou3d
+from AB3DMOT_libs.kalman_filter import KalmanBoxTracker
+
+
+TYPE_THRED = 10
+COLOR_THRED = 10
+LICENSE_PLATE_NUMBER_THRED = 10
+NAME_THRED = 10
+
+
+def associate_detections_to_trackers(detections, trackers, iou_threshold=0.01):
+    """
+    Assigns detections to tracked object (both represented as bounding boxes)
+
+    detections:  N x 8 x 3
+    trackers:    M x 8 x 3
+
+
+    Returns 3 lists of matches, unmatched_detections and unmatched_trackers
+    """
+    if (len(trackers) == 0):
+        return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 8, 3), dtype=int)
+    iou_matrix = np.zeros((len(detections), len(trackers)), dtype=np.float32)
+
+    for d, det in enumerate(detections):
+        for t, trk in enumerate(trackers):
+            iou_matrix[d, t] = iou3d(det, trk)[0]  # det: 8 x 3, trk: 8 x 3
+    # matched_indices = linear_assignment(-iou_matrix)
+    # hougarian algorithm, compatible to linear_assignment in sklearn.utils
+
+    row_ind, col_ind = linear_sum_assignment(-iou_matrix)  # hougarian algorithm
+    matched_indices = np.stack((row_ind, col_ind), axis=1)
+
+    unmatched_detections = []
+    for d, det in enumerate(detections):
+        if (d not in matched_indices[:, 0]): unmatched_detections.append(d)
+    unmatched_trackers = []
+    for t, trk in enumerate(trackers):
+        if (t not in matched_indices[:, 1]): unmatched_trackers.append(t)
+
+    # filter out matched with low IOU
+    matches = []
+    for m in matched_indices:
+        if (iou_matrix[m[0], m[1]] < iou_threshold):
+            unmatched_detections.append(m[0])
+            unmatched_trackers.append(m[1])
+        else:
+            matches.append(m.reshape(1, 2))
+    if (len(matches) == 0):
+        matches = np.empty((0, 2), dtype=int)
+    else:
+        matches = np.concatenate(matches, axis=0)
+
+    return matches, np.array(unmatched_detections), np.array(unmatched_trackers)
+
+
+class AB3DMOT(object):  # A baseline of 3D multi-object tracking
+    # max age will preserve the bbox does not appear no more than 2 frames, interpolate the detection
+    def __init__(self, max_age=2,
+                 min_hits=3):
+        """
+        Sets key parameters for SORT
+        """
+        self.max_age = max_age
+        self.min_hits = min_hits
+        self.trackers = []
+        self.frame_count = 0
+        self.reorder = [3, 4, 5, 6, 2, 1, 0]
+        self.reorder_back = [6, 5, 4, 0, 1, 2, 3]
+
+    def update(self, dets_all):
+        """
+        Params:
+          dets_all: dict
+            dets - a numpy array of detections in the format [[h,w,l,x,y,z,theta],...]
+            info: a array of other info for each det
+        Requires: this method must be called once for each frame even with empty detections.
+        Returns the a similar array, where the last column is the object ID.
+
+        NOTE: The number of objects returned may differ from the number of detections provided.
+        """
+        dets, info = dets_all['dets'], dets_all['info']  # dets: N x 7, float numpy array
+
+        # reorder the data to put x,y,z in front to be compatible with the state transition matrix
+        # where the constant velocity model is defined in the first three rows of the matrix
+        dets = dets[:, self.reorder]  # reorder the data to [[x,y,z,theta,l,w,h], ...]
+        self.frame_count += 1
+
+        trks = np.zeros((len(self.trackers), 7))  # N x 7 , # get predicted locations from existing trackers.
+        to_del = []
+        ret = []
+        for t, trk in enumerate(trks):
+            pos = self.trackers[t].predict().reshape((-1, 1))
+            trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]]
+            if (np.any(np.isnan(pos))):
+                to_del.append(t)
+        trks = np.ma.compress_rows(np.ma.masked_invalid(trks))
+        for t in reversed(to_del):
+            self.trackers.pop(t)
+
+        dets_8corner = [convert_3dbox_to_8corner(det_tmp) for det_tmp in dets]
+        if len(dets_8corner) > 0:
+            dets_8corner = np.stack(dets_8corner, axis=0)
+        else:
+            dets_8corner = []
+        trks_8corner = [convert_3dbox_to_8corner(trk_tmp) for trk_tmp in trks]
+        if len(trks_8corner) > 0: trks_8corner = np.stack(trks_8corner, axis=0)
+        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets_8corner, trks_8corner)
+
+        # update matched trackers with assigned detections
+        for t, trk in enumerate(self.trackers):
+            if t not in unmatched_trks:
+                d = matched[np.where(matched[:, 1] == t)[0], 0]  # a list of index
+                ori_info = trk.info
+                cur_info = info[d, :][0]
+
+                max_type = cur_info[1]
+                if len(trk.type_history) >= TYPE_THRED:
+                    max_type = max(trk.type_history, key=trk.type_history.count)
+                    del trk.type_history[0]
+                trk.type_history.append(cur_info[1])
+                cur_info[1] = max_type
+
+                max_color = cur_info[-1]
+                if len(trk.color_history) >= COLOR_THRED:
+                    max_color = max(trk.color_history, key=trk.color_history.count)
+                    del trk.color_history[0]
+                if cur_info[-1] != 'None':
+                    trk.color_history.append(cur_info[-1])
+                cur_info[-1] = max_color
+
+                max_license_plate_number_history = cur_info[-2]
+                if len(trk.license_plate_number_history) >= LICENSE_PLATE_NUMBER_THRED:
+                    max_license_plate_number_history = max(trk.license_plate_number_history, key=trk.license_plate_number_history.count)
+                    del trk.license_plate_number_history[0]
+                if cur_info[-2] != 'None':
+                    trk.license_plate_number_history.append(cur_info[-2])
+                cur_info[-2] = max_license_plate_number_history
+
+                max_name_history = cur_info[-3]
+                if len(trk.name_history) >= NAME_THRED:
+                    max_name_history = max(trk.name_history, key=trk.name_history.count)
+                    del trk.name_history[0]
+                if cur_info[-3] != 'None':
+                    trk.name_history.append(cur_info[-3])
+                cur_info[-3] = max_name_history
+
+                #if cur_info[1] == ori_info[1]:
+                #    if ori_info[-1] !='None':
+                #        cur_info[-1] = ori_info[-1]
+                #    if ori_info[-2] !='None':
+                #        cur_info[-2] = ori_info[-2]
+                #    if ori_info[-3] !='None':
+                #        cur_info[-3] = ori_info[-3]
+                trk.update(dets[d, :][0], cur_info)
+            else:
+                trk.info[0] = info[0][0]
+
+        # create and initialise new trackers for unmatched detections
+        for i in unmatched_dets:  # a scalar of index
+            trk = KalmanBoxTracker(dets[i, :], info[i, :])
+            self.trackers.append(trk)
+        i = len(self.trackers)
+        for trk in reversed(self.trackers):
+            d = trk.get_state()  # bbox location
+            d = d[self.reorder_back]  # change format from [x,y,z,theta,l,w,h] to [h,w,l,x,y,z,theta]
+
+            orientation = trk.get_orientation()
+
+            if ((trk.time_since_update < self.max_age) and (
+                    trk.hits >= self.min_hits or self.frame_count <= self.min_hits)):
+            # if trk.time_since_update < self.max_age:
+                ret.append(
+                    np.concatenate((d, [trk.id + 1], orientation, trk.info)).reshape(1, -1))  # +1 as MOT benchmark requires positive
+            i -= 1
+
+            # remove dead tracklet
+            if (trk.time_since_update >= self.max_age):
+                self.trackers.pop(i)
+        if (len(ret) > 0): return np.concatenate(ret)  # h,w,l,x,y,z,theta, ID, other info, confidence
+        return np.empty((0, 17))
+
+
+    def predict(self, timestamp):
+
+        rets = []
+
+        if len(self.trackers) == 0:
+            return rets
+        
+        pre_timestamp = int(self.trackers[0].info[0])
+        if (timestamp - pre_timestamp) > 450:
+            return rets
+
+        count = int(round((timestamp - pre_timestamp) / 100.0))
+        index = 1
+        while index < count:
+            self.frame_count += 1
+
+            trks = np.zeros((len(self.trackers), 7))
+            to_del = []
+            ret = []
+            for t, trk in enumerate(trks):
+                pos = self.trackers[t].predict().reshape((-1, 1))
+                trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]]
+                if (np.any(np.isnan(pos))):
+                    to_del.append(t)
+            for t in reversed(to_del):
+                self.trackers.pop(t)
+
+            for trk in reversed(self.trackers):
+                d = trk.get_state()  # bbox location
+                d = d[self.reorder_back]  # change format from [x,y,z,theta,l,w,h] to [h,w,l,x,y,z,theta]
+
+                orientation = trk.get_orientation()
+
+                trk.info[0] = pre_timestamp + 100*index
+                ret.append(
+                    np.concatenate((d, [trk.id + 1], orientation, trk.info)).reshape(1, -1))  # +1 as MOT benchmark requires positive
+
+            rets.append(np.concatenate(ret))
+            index += 1
+        return rets

tracking/scripts/listener.py

@@ -51,6 +51,7 @@ def rotz(t):
 def callback(msgs):
 
     log_file = open(log_file_path, 'a')
+    ori_log_file = open(ori_log_file_path, 'a')
 
     cloud_pub = rospy.Publisher("tracking_cloud", PointCloud2, queue_size=10)
     cloud_pub.publish(msgs.cloud)
@@ -77,6 +78,12 @@ def callback(msgs):
         info = [msg.frame, msg.type, msg.score, msg.name, msg.license_plate_number, msg.color_name]
         dets.append(det)
         infos.append(info)
+
+        ori_log_file.write('%u,%u,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%.8f,%.8f,%.8f,%.8f,%s,%s,%s\n' % (
+          msg.frame, msg.type, msg.score, msg.h, msg.w, msg.l, msg.x, msg.y, msg.z, msg.rot_y,
+          msg.obj_id, msg.v_x, msg.v_y, msg.v_z,msg.loc_x, msg.loc_y, msg.loc_z, 0, 0, 0, 0, msg.name, msg.license_plate_number, msg.color_name))
+    ori_log_file.close()
+
     dets_all = {'dets': np.asarray(dets), 'info': np.asarray(infos)}
 
     trackers = mot_tracker.update(dets_all)
@@ -203,7 +210,7 @@ def callback(msgs):
             t_marker.color.g = 1.0
             t_marker.color.b = 0
             t_marker.lifetime = rospy.Duration(0.5)
-            t_marker.text = 'id:{0},type:{1},v:{2:.2f}m/s,No:{3}'.format(msg.obj_id, msg.name, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.license_plate_number)
+            t_marker.text = 'id:{0},type:{1},type_name:{2},v:{3:.2f}m/s,No:{4}\ncolor_name:{5},frame:{6}'.format(msg.obj_id, msg.type, msg.name, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.license_plate_number, msg.color_name, msg.frame)
             marker_arr.markers.append(t_marker)
             i += 1
         
@@ -232,16 +239,20 @@ def listener():
 
 
 if __name__ == '__main__':
-    global mot_tracker, log_file_path
+    global mot_tracker, log_file_path, ori_log_file_path
     mot_tracker = AB3DMOT(max_age=3, min_hits=3)
     cur_time = time.strftime('%Y%m%d_%H:%M:%S', time.localtime(time.time()))
 
     log_file_path = 'src/tracking/logs/' + cur_time + '.txt'
-    if not os.path.exists('src/tracking/logs/'):
-        os.mkdir('src/tracking/logs/')
+    ori_log_file_path = 'src/tracking/logs/' + cur_time + '_ori.txt'
+    if not os.path.exists('src/tracking/logs'):
+        os.mkdir('src/tracking/logs')
     with open(log_file_path, 'w') as f:
         f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name\n")
 
+    with open(ori_log_file_path, 'w') as f:
+        f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name\n")
+
     listener()
 
 

tracking/scripts/listener_10fps.py

@@ -51,6 +51,7 @@ def rotz(t):
 def callback(msgs):
 
     log_file = open(log_file_path, 'a')
+    ori_log_file = open(ori_log_file_path, 'a')
 
     cloud_pub = rospy.Publisher("tracking_cloud", PointCloud2, queue_size=10)
     cloud_pub.publish(msgs.cloud)
@@ -111,6 +112,12 @@ def callback(msgs):
         info = [msg.frame, msg.type, msg.score, msg.name, msg.license_plate_number, msg.color_name]
         dets.append(det)
         infos.append(info)
+
+        ori_log_file.write('%u,%u,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%.8f,%.8f,%.8f,%.8f,%s,%s,%s\n' % (
+          msg.frame, msg.type, msg.score, msg.h, msg.w, msg.l, msg.x, msg.y, msg.z, msg.rot_y,
+          msg.obj_id, msg.v_x, msg.v_y, msg.v_z,msg.loc_x, msg.loc_y, msg.loc_z, 0, 0, 0, 0, msg.name, msg.license_plate_number, msg.color_name))
+    ori_log_file.close()
+
     dets_all = {'dets': np.asarray(dets), 'info': np.asarray(infos)}
 
     trackers = mot_tracker.update(dets_all)
@@ -237,7 +244,7 @@ def callback(msgs):
             t_marker.color.g = 1.0
             t_marker.color.b = 0
             t_marker.lifetime = rospy.Duration(0.5)
-            t_marker.text = 'id:{0},type:{1},v:{2:.2f}m/s,No:{3}'.format(msg.obj_id, msg.name, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.license_plate_number)
+            t_marker.text = 'id:{0},type:{1},type_name:{2},v:{3:.2f}m/s,No:{4}\ncolor_name:{5},frame:{6}'.format(msg.obj_id, msg.type, msg.name, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.license_plate_number, msg.color_name, msg.frame)
             marker_arr.markers.append(t_marker)
             i += 1
         
@@ -266,16 +273,20 @@ def listener():
 
 
 if __name__ == '__main__':
-    global mot_tracker, log_file_path
+    global mot_tracker, log_file_path, ori_log_file_path
     mot_tracker = AB3DMOT(max_age=3, min_hits=3)
     cur_time = time.strftime('%Y%m%d_%H:%M:%S', time.localtime(time.time()))
 
     log_file_path = 'src/tracking/logs/' + cur_time + '.txt'
-    if not os.path.exists('src/tracking/logs/'):
-        os.mkdir('src/tracking/logs/')
+    ori_log_file_path = 'src/tracking/logs/' + cur_time + '_ori.txt'
+    if not os.path.exists('src/tracking/logs'):
+        os.mkdir('src/tracking/logs')
     with open(log_file_path, 'w') as f:
         f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name\n")
 
+    with open(ori_log_file_path, 'w') as f:
+        f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name\n")
+
     listener()