diff --git a/boxmot/motion/cmc/cmc_interface.py b/boxmot/motion/cmc/base_cmc.py similarity index 97% rename from boxmot/motion/cmc/cmc_interface.py rename to boxmot/motion/cmc/base_cmc.py index 21b826c79c..ebb48291bc 100644 --- a/boxmot/motion/cmc/cmc_interface.py +++ b/boxmot/motion/cmc/base_cmc.py @@ -5,7 +5,7 @@ from abc import ABC, abstractmethod -class CMCInterface(ABC): +class BaseCMC(ABC): @abstractmethod def apply(self, im): diff --git a/boxmot/motion/cmc/ecc.py b/boxmot/motion/cmc/ecc.py index b96568b41c..7a02fce5ee 100644 --- a/boxmot/motion/cmc/ecc.py +++ b/boxmot/motion/cmc/ecc.py @@ -5,12 +5,12 @@ import cv2 import numpy as np -from boxmot.motion.cmc.cmc_interface import CMCInterface +from boxmot.motion.cmc.base_cmc import BaseCMC from boxmot.utils import BOXMOT from boxmot.utils import logger as LOGGER -class ECC(CMCInterface): +class ECC(BaseCMC): def __init__( self, warp_mode: int = cv2.MOTION_EUCLIDEAN, diff --git a/boxmot/motion/cmc/orb.py b/boxmot/motion/cmc/orb.py index 46173820c0..841b9a7e03 100644 --- a/boxmot/motion/cmc/orb.py +++ b/boxmot/motion/cmc/orb.py @@ -6,11 +6,11 @@ import cv2 import numpy as np -from boxmot.motion.cmc.cmc_interface import CMCInterface +from boxmot.motion.cmc.base_cmc import BaseCMC from boxmot.utils import BOXMOT -class ORB(CMCInterface): +class ORB(BaseCMC): def __init__( self, diff --git a/boxmot/motion/cmc/sift.py b/boxmot/motion/cmc/sift.py index b7e4694399..7c80e0a90f 100644 --- a/boxmot/motion/cmc/sift.py +++ b/boxmot/motion/cmc/sift.py @@ -6,11 +6,11 @@ import cv2 import numpy as np -from boxmot.motion.cmc.cmc_interface import CMCInterface +from boxmot.motion.cmc.base_cmc import BaseCMC from boxmot.utils import BOXMOT -class SIFT(CMCInterface): +class SIFT(BaseCMC): def __init__( self, diff --git a/boxmot/motion/cmc/sof.py b/boxmot/motion/cmc/sof.py index 8f0024ab0e..7f58324cbc 100644 --- a/boxmot/motion/cmc/sof.py +++ b/boxmot/motion/cmc/sof.py @@ -5,12 +5,12 @@ import cv2 import numpy as np -from boxmot.motion.cmc.cmc_interface import CMCInterface +from boxmot.motion.cmc.base_cmc import BaseCMC from boxmot.utils import BOXMOT from boxmot.utils import logger as LOGGER -class SOF(CMCInterface): +class SOF(BaseCMC): def __init__( self, diff --git a/boxmot/motion/kalman_filters/base_kalman_filter.py b/boxmot/motion/kalman_filters/base_kalman_filter.py new file mode 100644 index 0000000000..a20bd498aa --- /dev/null +++ b/boxmot/motion/kalman_filters/base_kalman_filter.py @@ -0,0 +1,158 @@ +import numpy as np +import scipy.linalg +from typing import Tuple + +""" +Table for the 0.95 quantile of the chi-square distribution with N degrees of +freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv +function and used as Mahalanobis gating threshold. +""" +chi2inv95 = { + 1: 3.8415, + 2: 5.9915, + 3: 7.8147, + 4: 9.4877, + 5: 11.070, + 6: 12.592, + 7: 14.067, + 8: 15.507, + 9: 16.919 +} + +class BaseKalmanFilter: + """ + Base class for Kalman filters tracking bounding boxes in image space. + """ + + def __init__(self, ndim: int): + self.ndim = ndim + self.dt = 1. + + # Create Kalman filter model matrices. + self._motion_mat = np.eye(2 * ndim, 2 * ndim) # State transition matrix + for i in range(ndim): + self._motion_mat[i, ndim + i] = self.dt + self._update_mat = np.eye(ndim, 2 * ndim) # Observation matrix + + # Motion and observation uncertainty weights. + self._std_weight_position = 1. / 20 + self._std_weight_velocity = 1. / 160 + + def initiate(self, measurement: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Create track from unassociated measurement. + """ + mean_pos = measurement + mean_vel = np.zeros_like(mean_pos) + mean = np.r_[mean_pos, mean_vel] + + std = self._get_initial_covariance_std(measurement) + covariance = np.diag(np.square(std)) + return mean, covariance + + def _get_initial_covariance_std(self, measurement: np.ndarray) -> np.ndarray: + """ + Return initial standard deviations for the covariance matrix. + Should be implemented by subclasses. + """ + raise NotImplementedError + + def predict(self, mean: np.ndarray, covariance: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Run Kalman filter prediction step. + """ + std_pos, std_vel = self._get_process_noise_std(mean) + motion_cov = np.diag(np.square(np.r_[std_pos, std_vel])) + + mean = np.dot(mean, self._motion_mat.T) + covariance = np.linalg.multi_dot(( + self._motion_mat, covariance, self._motion_mat.T)) + motion_cov + + return mean, covariance + + def _get_process_noise_std(self, mean: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Return standard deviations for process noise. + Should be implemented by subclasses. + """ + raise NotImplementedError + + def project(self, mean: np.ndarray, covariance: np.ndarray, confidence: float = 0.0) -> Tuple[np.ndarray, np.ndarray]: + """ + Project state distribution to measurement space. + """ + std = self._get_measurement_noise_std(mean, confidence) + + # NSA Kalman algorithm from GIAOTracker, which proposes a formula to + # adaptively calculate the noise covariance Rek: + # Rk = (1 − ck) Rk + # where Rk is the preset constant measurement noise covariance + # and ck is the detection confidence score at state k. Intuitively, + # the detection has a higher score ck when it has less noise, + # which results in a low Re. + std = [(1 - confidence) * x for x in std] + + 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 multi_predict(self, mean: np.ndarray, covariance: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Run Kalman filter prediction step (Vectorized version). + """ + std_pos, std_vel = self._get_multi_process_noise_std(mean) + sqr = np.square(np.r_[std_pos, std_vel]).T + + motion_cov = [np.diag(sqr[i]) for i in range(len(mean))] + motion_cov = np.asarray(motion_cov) + + mean = np.dot(mean, self._motion_mat.T) + left = np.dot(self._motion_mat, covariance).transpose((1, 0, 2)) + covariance = np.dot(left, self._motion_mat.T) + motion_cov + + return mean, covariance + + def update(self, mean: np.ndarray, covariance: np.ndarray, measurement: np.ndarray, confidence: float = 0.0) -> Tuple[np.ndarray, np.ndarray]: + """ + Run Kalman filter correction step. + """ + projected_mean, projected_cov = self.project(mean, covariance, confidence) + + 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 _get_multi_process_noise_std(self, mean: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Return standard deviations for process noise in vectorized form. + Should be implemented by subclasses. + """ + raise NotImplementedError + + def gating_distance(self, mean: np.ndarray, covariance: np.ndarray, measurements: np.ndarray, only_position: bool = False, metric: str = 'maha') -> np.ndarray: + """ + Compute gating distance between state distribution and measurements. + """ + mean, covariance = self.project(mean, covariance) + + if only_position: + mean, covariance = mean[:2], covariance[:2, :2] + measurements = measurements[:, :2] + + d = measurements - mean + if metric == 'gaussian': + return np.sum(d * d, axis=1) + elif metric == 'maha': + cholesky_factor = np.linalg.cholesky(covariance) + 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 + else: + raise ValueError('invalid distance metric') \ No newline at end of file diff --git a/boxmot/motion/kalman_filters/xyah_kf.py b/boxmot/motion/kalman_filters/xyah_kf.py index f1c4065355..7db546aeca 100644 --- a/boxmot/motion/kalman_filters/xyah_kf.py +++ b/boxmot/motion/kalman_filters/xyah_kf.py @@ -1,283 +1,73 @@ -# vim: expandtab:ts=4:sw=4 import numpy as np -import scipy.linalg -""" -Table for the 0.95 quantile of the chi-square distribution with N degrees of -freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv -function and used as Mahalanobis gating threshold. -""" -chi2inv95 = { - 1: 3.8415, - 2: 5.9915, - 3: 7.8147, - 4: 9.4877, - 5: 11.070, - 6: 12.592, - 7: 14.067, - 8: 15.507, - 9: 16.919} +from typing import Tuple +from boxmot.motion.kalman_filters.base_kalman_filter import BaseKalmanFilter -class KalmanFilterXYAH(object): +class KalmanFilterXYAH(BaseKalmanFilter): """ - A simple Kalman filter for tracking bounding boxes in image space. - - The 8-dimensional state space - + A Kalman filter for tracking bounding boxes in image space with state space: x, y, a, h, vx, vy, va, vh - - contains the bounding box center position (x, y), aspect ratio a, height h, - and their respective velocities. - - Object motion follows a constant velocity model. The bounding box location - (x, y, a, h) is taken as direct observation of the state space (linear - observation model). - """ def __init__(self): - ndim, dt = 4, 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) - - # Motion and observation uncertainty are chosen relative to the current - # state estimate. These weights control the amount of uncertainty in - # the model. This is a bit hacky. - self._std_weight_position = 1. / 20 - self._std_weight_velocity = 1. / 160 - - def initiate(self, measurement): - """Create track from unassociated measurement. - - Parameters - ---------- - measurement : ndarray - Bounding box coordinates (x, y, a, h) with center position (x, y), - aspect ratio a, and height h. - - Returns - ------- - (ndarray, ndarray) - Returns the mean vector (8 dimensional) and covariance matrix (8x8 - 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 = [ - 2 * self._std_weight_position * measurement[3], - 2 * self._std_weight_position * measurement[3], - 1e-2, - 2 * self._std_weight_position * measurement[3], - 10 * self._std_weight_velocity * measurement[3], - 10 * self._std_weight_velocity * measurement[3], - 1e-5, - 10 * self._std_weight_velocity * measurement[3]] - covariance = np.diag(np.square(std)) - return mean, covariance - - def predict(self, mean, covariance): - """Run Kalman filter prediction step. - - Parameters - ---------- - mean : ndarray - The 8 dimensional mean vector of the object state at the previous - time step. - covariance : ndarray - The 8x8 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. - - """ + super().__init__(ndim=4) + + def _get_initial_covariance_std(self, measurement: np.ndarray) -> np.ndarray: + # initial uncertainty in the aspect ratio is very low, + # suggesting that it is not expected to vary significantly. + return [ + 2 * self._std_weight_position * measurement[3], # x + 2 * self._std_weight_position * measurement[3], # y + 1e-2, # a (aspect ratio) + 2 * self._std_weight_position * measurement[3], # H + 10 * self._std_weight_velocity * measurement[3], # vx + 10 * self._std_weight_velocity * measurement[3], # vy + 1e-5, # va (aspect ration vel) + 10 * self._std_weight_velocity * measurement[3] # vh + ] + + def _get_process_noise_std(self, mean: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + # very small process noise standard deviation assigned to the + # aspect ratio state and its velocity. Suggests + # that the aspect ratio is expected to remain relatively constant + # over time. std_pos = [ self._std_weight_position * mean[3], self._std_weight_position * mean[3], 1e-2, - self._std_weight_position * mean[3]] + self._std_weight_position * mean[3] + ] std_vel = [ self._std_weight_velocity * mean[3], self._std_weight_velocity * mean[3], 1e-5, - self._std_weight_velocity * mean[3]] - 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, confidence=.0): - """Project state distribution to measurement space. - - Parameters - ---------- - mean : ndarray - The state's mean vector (8 dimensional array). - covariance : ndarray - The state's covariance matrix (8x8 dimensional). - confidence: (dyh) 检测框置信度 - Returns - ------- - (ndarray, ndarray) - Returns the projected mean and covariance matrix of the given state - estimate. - - """ - std = [ + self._std_weight_velocity * mean[3] + ] + return std_pos, std_vel + + def _get_measurement_noise_std(self, mean: np.ndarray, confidence: float) -> np.ndarray: + # small measurement noise standard deviation for + # aspect ratio state, indicating low expected measurement noise in + # the aspect ratio. + std_noise = [ self._std_weight_position * mean[3], self._std_weight_position * mean[3], 1e-1, - self._std_weight_position * mean[3]] + self._std_weight_position * mean[3] + ] + return std_noise - # NSA Kalman algorithm from GIAOTracker, which proposes a formula to - # adaptively calculate the noise covariance Rek: - # Rk = (1 − ck) Rk - # where Rk is the preset constant measurement noise covariance - # and ck is the detection confidence score at state k. Intuitively, - # the detection has a higher score ck when it has less noise, - # which results in a low Re. - std = [(1 - confidence) * x for x in std] - - 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 multi_predict(self, mean, covariance): - """Run Kalman filter prediction step (Vectorized version). - Parameters - ---------- - mean : ndarray - The Nx8 dimensional mean matrix of the object states at the previous - time step. - covariance : ndarray - The Nx8x8 dimensional covariance matrics of the object states 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. - """ + def _get_multi_process_noise_std(self, mean: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: std_pos = [ - self._std_weight_position * mean[:, 2], self._std_weight_position * mean[:, 3], - self._std_weight_position * mean[:, 2], - self._std_weight_position * mean[:, 3]] + self._std_weight_position * mean[:, 3], + 1e-2 * np.ones_like(mean[:, 3]), + self._std_weight_position * mean[:, 3] + ] std_vel = [ - self._std_weight_velocity * mean[:, 2], self._std_weight_velocity * mean[:, 3], - self._std_weight_velocity * mean[:, 2], - self._std_weight_velocity * mean[:, 3]] - sqr = np.square(np.r_[std_pos, std_vel]).T - - motion_cov = [] - for i in range(len(mean)): - motion_cov.append(np.diag(sqr[i])) - motion_cov = np.asarray(motion_cov) - - mean = np.dot(mean, self._motion_mat.T) - left = np.dot(self._motion_mat, covariance).transpose((1, 0, 2)) - covariance = np.dot(left, self._motion_mat.T) + motion_cov - - return mean, covariance - - def update(self, mean, covariance, measurement, confidence=.0): - """Run Kalman filter correction step. - - Parameters - ---------- - mean : ndarray - The predicted state's mean vector (8 dimensional). - covariance : ndarray - The state's covariance matrix (8x8 dimensional). - measurement : ndarray - The 4 dimensional measurement vector (x, y, a, h), where (x, y) - is the center position, a the aspect ratio, and h the height of the - bounding box. - confidence: (dyh)检测框置信度 - Returns - ------- - (ndarray, ndarray) - Returns the measurement-corrected state distribution. - - """ - projected_mean, projected_cov = self.project(mean, covariance, confidence) - - 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, measurements, - only_position=False, metric='maha'): - """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 (8 dimensional). - covariance : ndarray - Covariance of the state distribution (8x8 dimensional). - measurements : ndarray - An Nx4 dimensional matrix of N measurements, each in - format (x, y, a, h) where (x, y) is the bounding box center - position, a the aspect ratio, and h the height. - only_position : Optional[bool] - If True, distance computation is done with respect to the bounding - box center position only. - - Returns - ------- - ndarray - Returns an array of length N, where the i-th element contains the - squared Mahalanobis distance between (mean, covariance) and - `measurements[i]`. - - """ - mean, covariance = self.project(mean, covariance) - - if only_position: - mean, covariance = mean[:2], covariance[:2, :2] - measurements = measurements[:, :2] - - if metric == 'gaussian': - return np.sum(d * d, axis=1) - elif metric == 'maha': - cholesky_factor = np.linalg.cholesky(covariance) - d = measurements - 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 - else: - raise ValueError('invalid distance metric') \ No newline at end of file + self._std_weight_velocity * mean[:, 3], + 1e-5 * np.ones_like(mean[:, 3]), + self._std_weight_velocity * mean[:, 3] + ] + return std_pos, std_vel diff --git a/boxmot/motion/kalman_filters/xywh_kf.py b/boxmot/motion/kalman_filters/xywh_kf.py index e0b37eaa25..c0af94dda4 100644 --- a/boxmot/motion/kalman_filters/xywh_kf.py +++ b/boxmot/motion/kalman_filters/xywh_kf.py @@ -1,79 +1,19 @@ -# vim: expandtab:ts=4:sw=4 import numpy as np -import scipy.linalg +from typing import Tuple +from boxmot.motion.kalman_filters.base_kalman_filter import BaseKalmanFilter -""" -Table for the 0.95 quantile of the chi-square distribution with N degrees of -freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv -function and used as Mahalanobis gating threshold. -""" -chi2inv95 = { - 1: 3.8415, - 2: 5.9915, - 3: 7.8147, - 4: 9.4877, - 5: 11.070, - 6: 12.592, - 7: 14.067, - 8: 15.507, - 9: 16.919} - - -class KalmanFilterXYWH(object): +class KalmanFilterXYWH(BaseKalmanFilter): """ - A simple Kalman filter for tracking bounding boxes in image space. - - The 8-dimensional state space - + A Kalman filter for tracking bounding boxes in image space with state space: x, y, w, h, vx, vy, vw, vh - - contains the bounding box center position (x, y), width w, height h, - and their respective velocities. - - Object motion follows a constant velocity model. The bounding box location - (x, y, w, h) is taken as direct observation of the state space (linear - observation model). - """ def __init__(self): - ndim, dt = 4, 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) - - # Motion and observation uncertainty are chosen relative to the current - # state estimate. These weights control the amount of uncertainty in - # the model. This is a bit hacky. - self._std_weight_position = 1. / 20 - self._std_weight_velocity = 1. / 160 + super().__init__(ndim=4) - def initiate(self, measurement): - """Create track from unassociated measurement. - - Parameters - ---------- - measurement : ndarray - Bounding box coordinates (x, y, w, h) with center position (x, y), - width w, and height h. - - Returns - ------- - (ndarray, ndarray) - Returns the mean vector (8 dimensional) and covariance matrix (8x8 - 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 = [ + def _get_initial_covariance_std(self, measurement: np.ndarray) -> np.ndarray: + return [ 2 * self._std_weight_position * measurement[2], 2 * self._std_weight_position * measurement[3], 2 * self._std_weight_position * measurement[2], @@ -81,189 +21,44 @@ def initiate(self, measurement): 10 * self._std_weight_velocity * measurement[2], 10 * self._std_weight_velocity * measurement[3], 10 * self._std_weight_velocity * measurement[2], - 10 * self._std_weight_velocity * measurement[3]] - covariance = np.diag(np.square(std)) - return mean, covariance + 10 * self._std_weight_velocity * measurement[3] + ] - def predict(self, mean, covariance): - """Run Kalman filter prediction step. - - Parameters - ---------- - mean : ndarray - The 8 dimensional mean vector of the object state at the previous - time step. - covariance : ndarray - The 8x8 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. - - """ + def _get_process_noise_std(self, mean: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: std_pos = [ self._std_weight_position * mean[2], self._std_weight_position * mean[3], self._std_weight_position * mean[2], - self._std_weight_position * mean[3]] + self._std_weight_position * mean[3] + ] std_vel = [ self._std_weight_velocity * mean[2], self._std_weight_velocity * mean[3], self._std_weight_velocity * mean[2], - self._std_weight_velocity * mean[3]] - motion_cov = np.diag(np.square(np.r_[std_pos, std_vel])) - - mean = np.dot(mean, self._motion_mat.T) - covariance = np.linalg.multi_dot(( - self._motion_mat, covariance, self._motion_mat.T)) + motion_cov + self._std_weight_velocity * mean[3] + ] + return std_pos, std_vel - return mean, covariance - - def project(self, mean, covariance): - """Project state distribution to measurement space. - - Parameters - ---------- - mean : ndarray - The state's mean vector (8 dimensional array). - covariance : ndarray - The state's covariance matrix (8x8 dimensional). - - Returns - ------- - (ndarray, ndarray) - Returns the projected mean and covariance matrix of the given state - estimate. - - """ - std = [ + def _get_measurement_noise_std(self, mean: np.ndarray, confidence: float) -> np.ndarray: + std_noise = [ self._std_weight_position * mean[2], self._std_weight_position * mean[3], self._std_weight_position * mean[2], - self._std_weight_position * mean[3]] - 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 multi_predict(self, mean, covariance): - """Run Kalman filter prediction step (Vectorized version). - Parameters - ---------- - mean : ndarray - The Nx8 dimensional mean matrix of the object states at the previous - time step. - covariance : ndarray - The Nx8x8 dimensional covariance matrics of the object states 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. - """ + self._std_weight_position * mean[3] + ] + return std_noise + + def _get_multi_process_noise_std(self, mean: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: std_pos = [ self._std_weight_position * mean[:, 2], self._std_weight_position * mean[:, 3], self._std_weight_position * mean[:, 2], - self._std_weight_position * mean[:, 3]] + self._std_weight_position * mean[:, 3] + ] std_vel = [ self._std_weight_velocity * mean[:, 2], self._std_weight_velocity * mean[:, 3], self._std_weight_velocity * mean[:, 2], - self._std_weight_velocity * mean[:, 3]] - sqr = np.square(np.r_[std_pos, std_vel]).T - - motion_cov = [] - for i in range(len(mean)): - motion_cov.append(np.diag(sqr[i])) - motion_cov = np.asarray(motion_cov) - - mean = np.dot(mean, self._motion_mat.T) - left = np.dot(self._motion_mat, covariance).transpose((1, 0, 2)) - covariance = np.dot(left, self._motion_mat.T) + motion_cov - - return mean, covariance - - def update(self, mean, covariance, measurement): - """Run Kalman filter correction step. - - Parameters - ---------- - mean : ndarray - The predicted state's mean vector (8 dimensional). - covariance : ndarray - The state's covariance matrix (8x8 dimensional). - measurement : ndarray - The 4 dimensional measurement vector (x, y, w, h), where (x, y) - is the center position, w the width, and h the height of the - bounding box. - - 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, measurements, - only_position=False, metric='maha'): - """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 (8 dimensional). - covariance : ndarray - Covariance of the state distribution (8x8 dimensional). - measurements : ndarray - An Nx4 dimensional matrix of N measurements, each in - format (x, y, a, h) where (x, y) is the bounding box center - position, a the aspect ratio, and h the height. - only_position : Optional[bool] - If True, distance computation is done with respect to the bounding - box center position only. - Returns - ------- - ndarray - Returns an array of length N, where the i-th element contains the - squared Mahalanobis distance between (mean, covariance) and - `measurements[i]`. - """ - mean, covariance = self.project(mean, covariance) - if only_position: - mean, covariance = mean[:2], covariance[:2, :2] - measurements = measurements[:, :2] - - d = measurements - mean - if metric == 'gaussian': - return np.sum(d * d, axis=1) - elif metric == 'maha': - cholesky_factor = np.linalg.cholesky(covariance) - 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 - else: - raise ValueError('invalid distance metric') \ No newline at end of file + self._std_weight_velocity * mean[:, 3] + ] + return std_pos, std_vel