Skip to content

perf(kf): construct small diagonal covariances directly #578

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JESUSROYETH wants to merge 1 commit into
base: develop
Choose a base branch
from
+28 −12
Open

perf(kf): construct small diagonal covariances directly #578

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 14 additions & 6 deletions src/trackers/core/botsort/tracklet.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,14 @@
)


def _diagonal_matrix(values: list[float]) -> np.ndarray:
"""Construct a diagonal matrix directly from known values."""
size = len(values)
matrix = np.zeros((size, size))
matrix.flat[:: size + 1] = values
return matrix


class BoTSORTTracklet(BaseTracklet):
"""Tracklet for the BoT-SORT tracker.

Expand Down Expand Up @@ -65,7 +73,7 @@ def _build_process_noise(self, w: float, h: float) -> np.ndarray:
sp, sv = self._SIGMA_P, self._SIGMA_V
if isinstance(self.state_estimator, XCYCSRStateEstimator):
s = np.sqrt(max(w * h, 1e-6))
return np.diag(
return _diagonal_matrix(
[
(sp * w) ** 2,
(sp * h) ** 2,
Expand All @@ -76,7 +84,7 @@ def _build_process_noise(self, w: float, h: float) -> np.ndarray:
(sv * s) ** 2,
]
)
return np.diag(
return _diagonal_matrix(
[
(sp * w) ** 2,
(sp * h) ** 2,
Expand All @@ -94,8 +102,8 @@ def _build_measurement_noise(self, w: float, h: float) -> np.ndarray:
sm = self._SIGMA_M
if isinstance(self.state_estimator, XCYCSRStateEstimator):
s = np.sqrt(max(w * h, 1e-6))
return np.diag([(sm * w) ** 2, (sm * h) ** 2, (sm * s) ** 2, (sm * 1.0) ** 2])
return np.diag([(sm * w) ** 2, (sm * h) ** 2, (sm * w) ** 2, (sm * h) ** 2])
return _diagonal_matrix([(sm * w) ** 2, (sm * h) ** 2, (sm * s) ** 2, (sm * 1.0) ** 2])
return _diagonal_matrix([(sm * w) ** 2, (sm * h) ** 2, (sm * w) ** 2, (sm * h) ** 2])

def _set_scale_aware_noise(self, w: float, h: float) -> None:
"""Set the initial Q, R and P from the first detection's size."""
Expand All @@ -105,7 +113,7 @@ def _set_scale_aware_noise(self, w: float, h: float) -> None:

if isinstance(self.state_estimator, XCYCSRStateEstimator):
s = np.sqrt(max(w * h, 1e-6))
state_covariance = np.diag(
state_covariance = _diagonal_matrix(
[
(2 * sp * w) ** 2,
(2 * sp * h) ** 2,
Expand All @@ -117,7 +125,7 @@ def _set_scale_aware_noise(self, w: float, h: float) -> None:
]
)
else:
state_covariance = np.diag(
state_covariance = _diagonal_matrix(
[
(2 * sp * w) ** 2,
(2 * sp * h) ** 2,
Expand Down
20 changes: 14 additions & 6 deletions src/trackers/core/mcbyte/tracklet.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,14 @@
)


def _diagonal_matrix(values: list[float]) -> np.ndarray:
"""Construct a diagonal matrix directly from known values."""
size = len(values)
matrix = np.zeros((size, size))
matrix.flat[:: size + 1] = values
return matrix


class McByteTracklet(BaseTracklet):
"""Tracklet for the McByte tracker.

Expand Down Expand Up @@ -65,7 +73,7 @@ def _build_process_noise(self, w: float, h: float) -> np.ndarray:
sp, sv = self._SIGMA_P, self._SIGMA_V
if isinstance(self.state_estimator, XCYCSRStateEstimator):
s = np.sqrt(max(w * h, 1e-6))
return np.diag(
return _diagonal_matrix(
[
(sp * w) ** 2,
(sp * h) ** 2,
Expand All @@ -76,7 +84,7 @@ def _build_process_noise(self, w: float, h: float) -> np.ndarray:
(sv * s) ** 2,
]
)
return np.diag(
return _diagonal_matrix(
[
(sp * w) ** 2,
(sp * h) ** 2,
Expand All @@ -94,8 +102,8 @@ def _build_measurement_noise(self, w: float, h: float) -> np.ndarray:
sm = self._SIGMA_M
if isinstance(self.state_estimator, XCYCSRStateEstimator):
s = np.sqrt(max(w * h, 1e-6))
return np.diag([(sm * w) ** 2, (sm * h) ** 2, (sm * s) ** 2, (sm * 1.0) ** 2])
return np.diag([(sm * w) ** 2, (sm * h) ** 2, (sm * w) ** 2, (sm * h) ** 2])
return _diagonal_matrix([(sm * w) ** 2, (sm * h) ** 2, (sm * s) ** 2, (sm * 1.0) ** 2])
return _diagonal_matrix([(sm * w) ** 2, (sm * h) ** 2, (sm * w) ** 2, (sm * h) ** 2])

def _set_scale_aware_noise(self, w: float, h: float) -> None:
"""Set the initial Q, R and P from the first detection's size."""
Expand All @@ -105,7 +113,7 @@ def _set_scale_aware_noise(self, w: float, h: float) -> None:

if isinstance(self.state_estimator, XCYCSRStateEstimator):
s = np.sqrt(max(w * h, 1e-6))
state_covariance = np.diag(
state_covariance = _diagonal_matrix(
[
(2 * sp * w) ** 2,
(2 * sp * h) ** 2,
Expand All @@ -117,7 +125,7 @@ def _set_scale_aware_noise(self, w: float, h: float) -> None:
]
)
else:
state_covariance = np.diag(
state_covariance = _diagonal_matrix(
[
(2 * sp * w) ** 2,
(2 * sp * h) ** 2,
Expand Down
Loading