Local tmp

KalmanFilter.py

@@ -0,0 +1,179 @@
+import numpy as np
+import scipy.linalg
+
+class KalmanFilter(object):
+
+    def __init__(self):
+        ndim, dt = 2, 1.
+
+        # Create Kalman filter model matrices.
+        self._motion_mat = np.eye(2 * ndim, 2 * ndim)
+
+        for i in range(ndim):
+            self._motion_mat[i, ndim + i] = dt
+
+        self._update_mat = np.eye(ndim, 2 * ndim)
+
+        self._std_position = 0.200 
+        self._std_velocity = 0.0350
+
+    def initiate(self, measurement):
+        """Create track from unassociated measurement.
+
+        Parameters
+        ----------
+        measurement : ndarray
+            world coordinates of a box (x,y)
+
+        Returns
+        -------
+        (ndarray, ndarray)
+            Returns the mean vector (4 dimensional) and covariance matrix (4x4
+            dimensional) of the new track. Unobserved velocities are initialized
+            to 0 mean.
+
+        """
+        mean_pos = measurement
+        mean_vel = np.zeros_like(mean_pos)
+        mean = np.r_[mean_pos, mean_vel]
+
+        std = [
+            self._std_position,
+            self._std_position,
+            self._std_velocity,
+            self._std_velocity
+        ]
+
+        covariance = np.diag(np.square(std))
+
+        return mean, covariance
+
+    def predict(self, mean, covariance):
+        """Run Kalman filter prediction step.
+
+        Parameters
+        ----------
+        mean : ndarray
+            The 4 dimensional mean vector of the object state at the previous time step.
+        covariance : ndarray
+            The 4x4 dimensional covariance matrix of the object state at the previous time step.
+
+        Returns
+        -------
+        (ndarray, ndarray)
+            Returns the mean vector and covariance matrix of the predicted
+            state. Unobserved velocities are initialized to 0 mean.
+        """
+
+        std_pos = [
+            self._std_position,
+            self._std_position
+        ]
+
+        std_vel = [
+            self._std_velocity,
+            self._std_velocity
+        ]
+
+        motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))
+        mean = np.dot(self._motion_mat, mean)
+
+        covariance = np.linalg.multi_dot((self._motion_mat, covariance, self._motion_mat.T)) + motion_cov
+
+        return mean, covariance
+
+    def project(self, mean, covariance):
+        """Project state distribution to measurement space.
+
+        Parameters
+        ----------
+        mean : ndarray
+            The state's mean vector (4 dimensional array).
+        covariance : ndarray
+            The state's covariance matrix (4x4 dimensional).
+
+        Returns
+        -------
+        (ndarray, ndarray)
+            Returns the projected mean and covariance matrix of the given state
+            estimate.
+
+        """
+
+        std = [
+            self._std_position,
+            self._std_position
+        ]
+
+        innovation_cov = np.diag(np.square(std))
+
+        mean = np.dot(self._update_mat, mean)
+        covariance = np.linalg.multi_dot((
+            self._update_mat, covariance, self._update_mat.T))
+
+        return mean, covariance + innovation_cov
+
+    def update(self, mean, covariance, measurement):
+        """Run Kalman filter correction step.
+
+        Parameters
+        ----------
+        mean : ndarray
+            The predicted state's mean vector (4 dimensional).
+        covariance : ndarray
+            The state's covariance matrix (4x4 dimensional).
+        measurement : ndarray
+            world coordinates of a box (x, y)
+
+        Returns
+        -------
+        (ndarray, ndarray)
+            Returns the measurement-corrected state distribution.
+        """
+
+        projected_mean, projected_cov = self.project(mean, covariance)
+
+        chol_factor, lower = scipy.linalg.cho_factor(
+            projected_cov, lower=True, check_finite=False)
+        kalman_gain = scipy.linalg.cho_solve(
+            (chol_factor, lower), np.dot(covariance, self._update_mat.T).T,
+            check_finite=False).T
+
+        innovation = measurement - projected_mean
+
+        new_mean = mean + np.dot(innovation, kalman_gain.T)
+        new_covariance = covariance - np.linalg.multi_dot((
+            kalman_gain, projected_cov, kalman_gain.T))
+
+        return new_mean, new_covariance
+
+    def gating_distance(self, mean, covariance, pos):
+        """Compute gating distance between state distribution and measurements.
+
+        A suitable distance threshold can be obtained from `chi2inv95`. If
+        `only_position` is False, the chi-square distribution has 4 degrees of
+        freedom, otherwise 2.
+
+        Parameters
+        ----------
+        mean : ndarray
+            Mean vector over the state distribution (4 dimensional).
+        covariance : ndarray
+            Covariance of the state distribution (4x4 dimensional).
+        pos : ndarray
+
+        Returns
+        -------
+        ndarray
+        """
+
+        mean, covariance = self.project(mean, covariance)
+
+        cholesky_factor = np.linalg.cholesky(covariance)
+        d = pos - mean
+        z = scipy.linalg.solve_triangular(
+            cholesky_factor, d.T, lower=True, check_finite=False,
+            overwrite_b=True)
+        squared_maha = np.sum(z * z, axis=0)
+
+        return squared_maha

Track.py

@@ -0,0 +1,23 @@
+class OnlineTrack:
+    def __init__(self, id, mean, covariance, kf):
+        """
+            each Track object has mean [x,y,dx,dy] and covariance matrix
+            and receives Kalman Filter object
+        """
+        self.id = id
+        self.mean = mean
+        self.covariance = covariance
+        self.kf = kf
+
+    def update(self, detection):
+        """
+            updates corresponding Kalman Filter object's parameters & mean and covariance of the track
+            detection - is a box in x,y,w,h format
+        """
+        self.mean, self.covariance = self.kf.update(self.mean, self.covariance, detection)
+
+    def predict(self):
+        """
+            updates track's mean and covariance
+        """
+        self.mean, self.covariance = self.kf.predict(self.mean, self.covariance)

Tracker.py

@@ -0,0 +1,71 @@
+from Track import OnlineTrack
+import numpy as np
+import KalmanFilter
+
+from scipy.optimize import linear_sum_assignment
+import utils
+
+class OnlineTracker:
+
+    def __init__(self):
+        self.tracks = []
+        self._next_id = 1
+
+    def onlineTrackerInit(self, boxes_init):
+
+        for box in boxes_init:
+            new_kf = KalmanFilter.KalmanFilter()
+            mean, cov = new_kf.initiate(np.asarray(box.to_world()))
+            self.tracks.append(OnlineTrack(self._next_id, mean, cov, new_kf))
+            self._next_id += 1
+
+        # print ('%d tracks created' % (len(self.tracks)))
+
+        return
+
+    def onlineTrackerAssign(self, measurements, fnum, thresh=5.9915, inf=1000000):
+
+        for track in self.tracks:
+            track.predict() # updates track.mean and track.covariance
+
+        cost_matrix = np.zeros((len(self.tracks), len(measurements)))
+
+        for i, track in enumerate(self.tracks):
+            for j, box in enumerate(measurements):
+                sq_mah_dist = track.kf.gating_distance(track.mean, track.covariance, box.to_world())
+                cost_matrix[i][j] = inf if sq_mah_dist >= thresh else sq_mah_dist
+
+        row_indices, col_indices = linear_sum_assignment(cost_matrix)
+
+        matches = []
+        unmatched_tracks = []
+        unmatched_dets = []
+
+        for row, col in zip(row_indices, col_indices):
+            if cost_matrix[row][col] == inf:
+                # print ('f:%d id:%d cost:%f' % (fnum, self.tracks[row].id, cost_matrix[row][col]))
+                unmatched_tracks.append(row)
+                unmatched_dets.append(col)
+            else:
+                matches.append((row, col))
+
+        for i, _ in enumerate(self.tracks):
+            if i not in row_indices:
+                unmatched_tracks.append(i)
+
+        for j, _ in enumerate(measurements):
+            if j not in col_indices:
+                unmatched_dets.append(j)
+
+        # for tidx in unmatched_tracks:
+        #     print ('f:%d id:%d unmatched' % (fnum, self.tracks[tidx].id))
+
+        return matches, unmatched_tracks, unmatched_dets
+
+    def onlineTrackerUpdate(self, matches, measurements):
+        """
+            tracks.update - new_measurements
+        """
+
+        for match in matches:
+            self.tracks[match[0]].update(measurements[match[1]].to_world())

TrackletNode.py

@@ -0,0 +1,7 @@
+class TrackletNode:
+    def __init__(self, idx, sfIdx, s3dc, efIdx, e3dc):
+        self._id = idx + 1
+        self._sfIdx = sfIdx
+        self._efIdx = efIdx
+        self._s3dc = s3dc
+        self._e3dc = e3dc

bbox.py

@@ -0,0 +1,30 @@
+import numpy as np
+
+class Box(object):
+    def __init__(self, tlbr, confidence, transform, imsize):
+        self.tlbr = np.asarray(tlbr, dtype=np.float)
+        self.confidence = float(confidence)
+        self.transform = transform
+        self.size = imsize
+
+    def to_tlwh(self):
+        tlbr = self.tlbr.copy()
+        w = tlbr[2]-tlbr[0]
+        h = tlbr[3]-tlbr[1]
+        x, y = tlbr[0], tlbr[1]
+        tlwh = [x,y,w,h]
+        return tlwh
+
+    def box2midpoint_normalised(self, box, iw, ih):
+        w = box[2]-box[0]
+        x, y = box[0] + w/2, box[3]
+        return (x/iw, y/ih)
+
+    def to_world(self):
+        p = self.box2midpoint_normalised(self.tlbr, self.size[1], self.size[0])
+        cx, cy = self.transform.video_to_ground(p[0], p[1])
+        cx, cy = cx*self.transform.parameter.get("ground_width"), cy*self.transform.parameter.get("ground_height")
+        return np.asarray([cx,cy], dtype=np.float)
+
+
+