00001
00002 import numpy as np
00003 import scipy.linalg
00004
00005
00006 """
00007 Table for the 0.95 quantile of the chi-square distribution with N degrees of
00008 freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv
00009 function and used as Mahalanobis gating threshold.
00010 """
00011 chi2inv95 = {
00012 1: 3.8415,
00013 2: 5.9915,
00014 3: 7.8147,
00015 4: 9.4877,
00016 5: 11.070,
00017 6: 12.592,
00018 7: 14.067,
00019 8: 15.507,
00020 9: 16.919}
00021
00022
00023 class KalmanFilter(object):
00024 """
00025 A simple Kalman filter for tracking bounding boxes in image space.
00026
00027 The 8-dimensional state space
00028
00029 x, y, a, h, vx, vy, va, vh
00030
00031 contains the bounding box center position (x, y), aspect ratio a, height h,
00032 and their respective velocities.
00033
00034 Object motion follows a constant velocity model. The bounding box location
00035 (x, y, a, h) is taken as direct observation of the state space (linear
00036 observation model).
00037
00038 """
00039
00040 def __init__(self):
00041 ndim, dt = 4, 1.
00042
00043
00044 self._motion_mat = np.eye(2 * ndim, 2 * ndim)
00045 for i in range(ndim):
00046 self._motion_mat[i, ndim + i] = dt
00047 self._update_mat = np.eye(ndim, 2 * ndim)
00048
00049
00050
00051
00052 self._std_weight_position = 1. / 20
00053 self._std_weight_velocity = 1. / 160
00054
00055 def initiate(self, measurement):
00056 """Create track from unassociated measurement.
00057
00058 Parameters
00059 ----------
00060 measurement : ndarray
00061 Bounding box coordinates (x, y, a, h) with center position (x, y),
00062 aspect ratio a, and height h.
00063
00064 Returns
00065 -------
00066 (ndarray, ndarray)
00067 Returns the mean vector (8 dimensional) and covariance matrix (8x8
00068 dimensional) of the new track. Unobserved velocities are initialized
00069 to 0 mean.
00070
00071 """
00072 mean_pos = measurement
00073 mean_vel = np.zeros_like(mean_pos)
00074 mean = np.r_[mean_pos, mean_vel]
00075
00076 std = [
00077 2 * self._std_weight_position * measurement[3],
00078 2 * self._std_weight_position * measurement[3],
00079 1e-2,
00080 2 * self._std_weight_position * measurement[3],
00081 10 * self._std_weight_velocity * measurement[3],
00082 10 * self._std_weight_velocity * measurement[3],
00083 1e-5,
00084 10 * self._std_weight_velocity * measurement[3]]
00085 covariance = np.diag(np.square(std))
00086 return mean, covariance
00087
00088 def predict(self, mean, covariance):
00089 """Run Kalman filter prediction step.
00090
00091 Parameters
00092 ----------
00093 mean : ndarray
00094 The 8 dimensional mean vector of the object state at the previous
00095 time step.
00096 covariance : ndarray
00097 The 8x8 dimensional covariance matrix of the object state at the
00098 previous time step.
00099
00100 Returns
00101 -------
00102 (ndarray, ndarray)
00103 Returns the mean vector and covariance matrix of the predicted
00104 state. Unobserved velocities are initialized to 0 mean.
00105
00106 """
00107 std_pos = [
00108 self._std_weight_position * mean[3],
00109 self._std_weight_position * mean[3],
00110 1e-2,
00111 self._std_weight_position * mean[3]]
00112 std_vel = [
00113 self._std_weight_velocity * mean[3],
00114 self._std_weight_velocity * mean[3],
00115 1e-5,
00116 self._std_weight_velocity * mean[3]]
00117 motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))
00118
00119 mean = np.dot(self._motion_mat, mean)
00120 covariance = np.linalg.multi_dot((
00121 self._motion_mat, covariance, self._motion_mat.T)) + motion_cov
00122
00123 return mean, covariance
00124
00125 def project(self, mean, covariance):
00126 """Project state distribution to measurement space.
00127
00128 Parameters
00129 ----------
00130 mean : ndarray
00131 The state's mean vector (8 dimensional array).
00132 covariance : ndarray
00133 The state's covariance matrix (8x8 dimensional).
00134
00135 Returns
00136 -------
00137 (ndarray, ndarray)
00138 Returns the projected mean and covariance matrix of the given state
00139 estimate.
00140
00141 """
00142 std = [
00143 self._std_weight_position * mean[3],
00144 self._std_weight_position * mean[3],
00145 1e-1,
00146 self._std_weight_position * mean[3]]
00147 innovation_cov = np.diag(np.square(std))
00148
00149 mean = np.dot(self._update_mat, mean)
00150 covariance = np.linalg.multi_dot((
00151 self._update_mat, covariance, self._update_mat.T))
00152 return mean, covariance + innovation_cov
00153
00154 def update(self, mean, covariance, measurement):
00155 """Run Kalman filter correction step.
00156
00157 Parameters
00158 ----------
00159 mean : ndarray
00160 The predicted state's mean vector (8 dimensional).
00161 covariance : ndarray
00162 The state's covariance matrix (8x8 dimensional).
00163 measurement : ndarray
00164 The 4 dimensional measurement vector (x, y, a, h), where (x, y)
00165 is the center position, a the aspect ratio, and h the height of the
00166 bounding box.
00167
00168 Returns
00169 -------
00170 (ndarray, ndarray)
00171 Returns the measurement-corrected state distribution.
00172
00173 """
00174 projected_mean, projected_cov = self.project(mean, covariance)
00175
00176 chol_factor, lower = scipy.linalg.cho_factor(
00177 projected_cov, lower=True, check_finite=False)
00178 kalman_gain = scipy.linalg.cho_solve(
00179 (chol_factor, lower), np.dot(covariance, self._update_mat.T).T,
00180 check_finite=False).T
00181 innovation = measurement - projected_mean
00182
00183 new_mean = mean + np.dot(innovation, kalman_gain.T)
00184 new_covariance = covariance - np.linalg.multi_dot((
00185 kalman_gain, projected_cov, kalman_gain.T))
00186 return new_mean, new_covariance
00187
00188 def gating_distance(self, mean, covariance, measurements,
00189 only_position=False):
00190 """Compute gating distance between state distribution and measurements.
00191
00192 A suitable distance threshold can be obtained from `chi2inv95`. If
00193 `only_position` is False, the chi-square distribution has 4 degrees of
00194 freedom, otherwise 2.
00195
00196 Parameters
00197 ----------
00198 mean : ndarray
00199 Mean vector over the state distribution (8 dimensional).
00200 covariance : ndarray
00201 Covariance of the state distribution (8x8 dimensional).
00202 measurements : ndarray
00203 An Nx4 dimensional matrix of N measurements, each in
00204 format (x, y, a, h) where (x, y) is the bounding box center
00205 position, a the aspect ratio, and h the height.
00206 only_position : Optional[bool]
00207 If True, distance computation is done with respect to the bounding
00208 box center position only.
00209
00210 Returns
00211 -------
00212 ndarray
00213 Returns an array of length N, where the i-th element contains the
00214 squared Mahalanobis distance between (mean, covariance) and
00215 `measurements[i]`.
00216
00217 """
00218 mean, covariance = self.project(mean, covariance)
00219 if only_position:
00220 mean, covariance = mean[:2], covariance[:2, :2]
00221 measurements = measurements[:, :2]
00222
00223 cholesky_factor = np.linalg.cholesky(covariance)
00224 d = measurements - mean
00225 z = scipy.linalg.solve_triangular(
00226 cholesky_factor, d.T, lower=True, check_finite=False,
00227 overwrite_b=True)
00228 squared_maha = np.sum(z * z, axis=0)
00229 return squared_maha