|
| 1 | +# vim: expandtab:ts=4:sw=4 |
| 2 | +import numpy as np |
| 3 | +import scipy.linalg |
| 4 | + |
| 5 | + |
| 6 | +""" |
| 7 | +Table for the 0.95 quantile of the chi-square distribution with N degrees of |
| 8 | +freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv |
| 9 | +function and used as Mahalanobis gating threshold. |
| 10 | +""" |
| 11 | +chi2inv95 = { |
| 12 | + 1: 3.8415, |
| 13 | + 2: 5.9915, |
| 14 | + 3: 7.8147, |
| 15 | + 4: 9.4877, |
| 16 | + 5: 11.070, |
| 17 | + 6: 12.592, |
| 18 | + 7: 14.067, |
| 19 | + 8: 15.507, |
| 20 | + 9: 16.919} |
| 21 | + |
| 22 | + |
| 23 | +class KalmanFilter(object): |
| 24 | + """ |
| 25 | + A simple Kalman filter for tracking bounding boxes in image space. |
| 26 | +
|
| 27 | + The 8-dimensional state space |
| 28 | +
|
| 29 | + x, y, a, h, vx, vy, va, vh |
| 30 | +
|
| 31 | + contains the bounding box center position (x, y), aspect ratio a, height h, |
| 32 | + and their respective velocities. |
| 33 | +
|
| 34 | + Object motion follows a constant velocity model. The bounding box location |
| 35 | + (x, y, a, h) is taken as direct observation of the state space (linear |
| 36 | + observation model). |
| 37 | +
|
| 38 | + """ |
| 39 | + |
| 40 | + def __init__(self): |
| 41 | + ndim, dt = 4, 1. |
| 42 | + |
| 43 | + # Create Kalman filter model matrices. |
| 44 | + self._motion_mat = np.eye(2 * ndim, 2 * ndim) |
| 45 | + for i in range(ndim): |
| 46 | + self._motion_mat[i, ndim + i] = dt |
| 47 | + self._update_mat = np.eye(ndim, 2 * ndim) |
| 48 | + |
| 49 | + # Motion and observation uncertainty are chosen relative to the current |
| 50 | + # state estimate. These weights control the amount of uncertainty in |
| 51 | + # the model. This is a bit hacky. |
| 52 | + self._std_weight_position = 1. / 20 |
| 53 | + self._std_weight_velocity = 1. / 160 |
| 54 | + |
| 55 | + def initiate(self, measurement): |
| 56 | + """Create track from unassociated measurement. |
| 57 | +
|
| 58 | + Parameters |
| 59 | + ---------- |
| 60 | + measurement : ndarray |
| 61 | + Bounding box coordinates (x, y, a, h) with center position (x, y), |
| 62 | + aspect ratio a, and height h. |
| 63 | +
|
| 64 | + Returns |
| 65 | + ------- |
| 66 | + (ndarray, ndarray) |
| 67 | + Returns the mean vector (8 dimensional) and covariance matrix (8x8 |
| 68 | + dimensional) of the new track. Unobserved velocities are initialized |
| 69 | + to 0 mean. |
| 70 | +
|
| 71 | + """ |
| 72 | + mean_pos = measurement |
| 73 | + mean_vel = np.zeros_like(mean_pos) |
| 74 | + mean = np.r_[mean_pos, mean_vel] |
| 75 | + |
| 76 | + std = [ |
| 77 | + 2 * self._std_weight_position * measurement[3], |
| 78 | + 2 * self._std_weight_position * measurement[3], |
| 79 | + 1e-2, |
| 80 | + 2 * self._std_weight_position * measurement[3], |
| 81 | + 10 * self._std_weight_velocity * measurement[3], |
| 82 | + 10 * self._std_weight_velocity * measurement[3], |
| 83 | + 1e-5, |
| 84 | + 10 * self._std_weight_velocity * measurement[3]] |
| 85 | + covariance = np.diag(np.square(std)) |
| 86 | + return mean, covariance |
| 87 | + |
| 88 | + def predict(self, mean, covariance): |
| 89 | + """Run Kalman filter prediction step. |
| 90 | +
|
| 91 | + Parameters |
| 92 | + ---------- |
| 93 | + mean : ndarray |
| 94 | + The 8 dimensional mean vector of the object state at the previous |
| 95 | + time step. |
| 96 | + covariance : ndarray |
| 97 | + The 8x8 dimensional covariance matrix of the object state at the |
| 98 | + previous time step. |
| 99 | +
|
| 100 | + Returns |
| 101 | + ------- |
| 102 | + (ndarray, ndarray) |
| 103 | + Returns the mean vector and covariance matrix of the predicted |
| 104 | + state. Unobserved velocities are initialized to 0 mean. |
| 105 | +
|
| 106 | + """ |
| 107 | + std_pos = [ |
| 108 | + self._std_weight_position * mean[3], |
| 109 | + self._std_weight_position * mean[3], |
| 110 | + 1e-2, |
| 111 | + self._std_weight_position * mean[3]] |
| 112 | + std_vel = [ |
| 113 | + self._std_weight_velocity * mean[3], |
| 114 | + self._std_weight_velocity * mean[3], |
| 115 | + 1e-5, |
| 116 | + self._std_weight_velocity * mean[3]] |
| 117 | + motion_cov = np.diag(np.square(np.r_[std_pos, std_vel])) |
| 118 | + |
| 119 | + mean = np.dot(self._motion_mat, mean) |
| 120 | + covariance = np.linalg.multi_dot(( |
| 121 | + self._motion_mat, covariance, self._motion_mat.T)) + motion_cov |
| 122 | + |
| 123 | + return mean, covariance |
| 124 | + |
| 125 | + def project(self, mean, covariance): |
| 126 | + """Project state distribution to measurement space. |
| 127 | +
|
| 128 | + Parameters |
| 129 | + ---------- |
| 130 | + mean : ndarray |
| 131 | + The state's mean vector (8 dimensional array). |
| 132 | + covariance : ndarray |
| 133 | + The state's covariance matrix (8x8 dimensional). |
| 134 | +
|
| 135 | + Returns |
| 136 | + ------- |
| 137 | + (ndarray, ndarray) |
| 138 | + Returns the projected mean and covariance matrix of the given state |
| 139 | + estimate. |
| 140 | +
|
| 141 | + """ |
| 142 | + std = [ |
| 143 | + self._std_weight_position * mean[3], |
| 144 | + self._std_weight_position * mean[3], |
| 145 | + 1e-1, |
| 146 | + self._std_weight_position * mean[3]] |
| 147 | + innovation_cov = np.diag(np.square(std)) |
| 148 | + |
| 149 | + mean = np.dot(self._update_mat, mean) |
| 150 | + covariance = np.linalg.multi_dot(( |
| 151 | + self._update_mat, covariance, self._update_mat.T)) |
| 152 | + return mean, covariance + innovation_cov |
| 153 | + |
| 154 | + def update(self, mean, covariance, measurement): |
| 155 | + """Run Kalman filter correction step. |
| 156 | +
|
| 157 | + Parameters |
| 158 | + ---------- |
| 159 | + mean : ndarray |
| 160 | + The predicted state's mean vector (8 dimensional). |
| 161 | + covariance : ndarray |
| 162 | + The state's covariance matrix (8x8 dimensional). |
| 163 | + measurement : ndarray |
| 164 | + The 4 dimensional measurement vector (x, y, a, h), where (x, y) |
| 165 | + is the center position, a the aspect ratio, and h the height of the |
| 166 | + bounding box. |
| 167 | +
|
| 168 | + Returns |
| 169 | + ------- |
| 170 | + (ndarray, ndarray) |
| 171 | + Returns the measurement-corrected state distribution. |
| 172 | +
|
| 173 | + """ |
| 174 | + projected_mean, projected_cov = self.project(mean, covariance) |
| 175 | + |
| 176 | + chol_factor, lower = scipy.linalg.cho_factor( |
| 177 | + projected_cov, lower=True, check_finite=False) |
| 178 | + kalman_gain = scipy.linalg.cho_solve( |
| 179 | + (chol_factor, lower), np.dot(covariance, self._update_mat.T).T, |
| 180 | + check_finite=False).T |
| 181 | + innovation = measurement - projected_mean |
| 182 | + |
| 183 | + new_mean = mean + np.dot(innovation, kalman_gain.T) |
| 184 | + new_covariance = covariance - np.linalg.multi_dot(( |
| 185 | + kalman_gain, projected_cov, kalman_gain.T)) |
| 186 | + return new_mean, new_covariance |
| 187 | + |
| 188 | + def gating_distance(self, mean, covariance, measurements, |
| 189 | + only_position=False): |
| 190 | + """Compute gating distance between state distribution and measurements. |
| 191 | +
|
| 192 | + A suitable distance threshold can be obtained from `chi2inv95`. If |
| 193 | + `only_position` is False, the chi-square distribution has 4 degrees of |
| 194 | + freedom, otherwise 2. |
| 195 | +
|
| 196 | + Parameters |
| 197 | + ---------- |
| 198 | + mean : ndarray |
| 199 | + Mean vector over the state distribution (8 dimensional). |
| 200 | + covariance : ndarray |
| 201 | + Covariance of the state distribution (8x8 dimensional). |
| 202 | + measurements : ndarray |
| 203 | + An Nx4 dimensional matrix of N measurements, each in |
| 204 | + format (x, y, a, h) where (x, y) is the bounding box center |
| 205 | + position, a the aspect ratio, and h the height. |
| 206 | + only_position : Optional[bool] |
| 207 | + If True, distance computation is done with respect to the bounding |
| 208 | + box center position only. |
| 209 | +
|
| 210 | + Returns |
| 211 | + ------- |
| 212 | + ndarray |
| 213 | + Returns an array of length N, where the i-th element contains the |
| 214 | + squared Mahalanobis distance between (mean, covariance) and |
| 215 | + `measurements[i]`. |
| 216 | +
|
| 217 | + """ |
| 218 | + mean, covariance = self.project(mean, covariance) |
| 219 | + if only_position: |
| 220 | + mean, covariance = mean[:2], covariance[:2, :2] |
| 221 | + measurements = measurements[:, :2] |
| 222 | + |
| 223 | + cholesky_factor = np.linalg.cholesky(covariance) |
| 224 | + d = measurements - mean |
| 225 | + z = scipy.linalg.solve_triangular( |
| 226 | + cholesky_factor, d.T, lower=True, check_finite=False, |
| 227 | + overwrite_b=True) |
| 228 | + squared_maha = np.sum(z * z, axis=0) |
| 229 | + return squared_maha |
0 commit comments