From c5c9857e3ce56f5593b4e0220e1c609a7b6e4ce2 Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Fri, 8 May 2026 17:35:48 +0300 Subject: [PATCH 1/9] feat(BoTSORT): track() overload accepting pre-computed embeddings Add a second public BoTSORT::track() entry point that accepts a parallel vector of FeatureVector embeddings alongside the detections. When supplied, the embeddings are used verbatim to construct each Track and to drive the appearance-aware association branches, regardless of whether the legacy TrackerParams::reid_enabled flag is set or an internal ReIDModel was loaded. This lets a downstream pipeline that already runs Re-ID on GPU (e.g. an upstream OSNet stage) feed the tracker without forcing the internal _extract_features() path, which round-trips each detection through host cv::Mat patches. Behaviour preservation: - The legacy track(detections, frame) overload is now a thin delegator that calls the new overload with an empty features vector. All existing semantics (low-conf segregation, KF predict, GMC, two-stage association, lost-track aging, duplicate removal) are unchanged. - TrackerParams::reid_enabled retains its original meaning: it gates internal _extract_features() only. - Empty features + reid_enabled -> internal extraction (legacy path). - Empty features + !reid_enabled -> motion-only tracking (legacy path). - Non-empty features -> appearance-aware association using the supplied embeddings; the internal model is not required. - Mismatched features.size() vs detections.size() throws std::invalid_argument. A cached _distance_metric member replaces the prior _reid_model->get_distance_metric() call sites; it is populated from the loaded model when present, otherwise defaults to "cosine" so external embeddings have a metric to use without an internal model. Co-Authored-By: Claude Opus 4.7 (1M context) --- botsort/include/BoTSORT.h | 35 +++++++++++++++++++-- botsort/src/BoTSORT.cpp | 65 +++++++++++++++++++++++++++++++++------ 2 files changed, 88 insertions(+), 12 deletions(-) diff --git a/botsort/include/BoTSORT.h b/botsort/include/BoTSORT.h index 684eb20..cd29cf5 100755 --- a/botsort/include/BoTSORT.h +++ b/botsort/include/BoTSORT.h @@ -26,15 +26,42 @@ class BoTSORT /** * @brief Track the objects in the frame - * + * * @param detections Detections in the frame * @param frame Frame - * @return std::vector> + * @return std::vector> */ std::vector> track(const std::vector &detections, const cv::Mat &frame); + /** + * @brief Track the objects in the frame using pre-computed appearance + * embeddings supplied by an external Re-ID stage. + * + * The supplied features are treated as parallel to @p detections — i.e. + * @c features[i] is the embedding for @c detections[i]. When @p features + * is non-empty, appearance-aware association is enabled regardless of + * the @c TrackerParams::reid_enabled flag (and regardless of whether an + * internal Re-ID model was successfully loaded). When @p features is + * empty, behaviour matches the (detections, frame) overload: internal + * extraction runs only if Re-ID was configured + loaded; otherwise + * motion-only tracking is performed. + * + * @param detections Detections in the frame. + * @param features Pre-computed embeddings, parallel to @p detections. + * Pass an empty vector to fall back to internal Re-ID + * (or motion-only tracking when Re-ID is disabled). + * @param frame Frame; used for clipping bbox extents and GMC. + * @return Active tracks for this frame. + * @throws std::invalid_argument if @p features is non-empty but its + * size differs from @p detections.size(). + */ + std::vector> + track(const std::vector &detections, + const std::vector &features, const cv::Mat &frame); + + private: /** * @brief Extract visual features from the given frame and bounding box @@ -95,6 +122,10 @@ class BoTSORT private: std::string _gmc_method_name; + // Distance metric used for embedding-based association. Populated from + // the loaded ReID model (if any) at construction; otherwise defaults to + // "cosine" so externally-supplied embeddings still have a metric to use. + std::string _distance_metric; bool _reid_enabled, _gmc_enabled; uint8_t _track_buffer, _frame_rate, _buffer_size, _max_time_lost; float _track_high_thresh, _track_low_thresh, _new_track_thresh, diff --git a/botsort/src/BoTSORT.cpp b/botsort/src/BoTSORT.cpp index e6c84f1..2059750 100755 --- a/botsort/src/BoTSORT.cpp +++ b/botsort/src/BoTSORT.cpp @@ -79,6 +79,13 @@ BoTSORT::BoTSORT(const Config &tracker_config, _reid_enabled = false; } + // Cache the distance metric. If a ReID model was loaded, defer to it; + // otherwise default to "cosine" so callers supplying externally-computed + // embeddings (via the track-with-features overload) still get a sensible + // metric without needing the internal model. + _distance_metric = + _reid_model ? _reid_model->get_distance_metric() : "cosine"; + // Global motion compensation module if (_gmc_enabled && not_empty(gmc_config)) { @@ -100,8 +107,37 @@ BoTSORT::BoTSORT(const Config &tracker_config, std::vector> BoTSORT::track(const std::vector &detections, const cv::Mat &frame) +{ + // Delegate to the features-aware overload with an empty feature vector; + // it preserves the legacy behaviour (internal ReID extraction when + // _reid_enabled, motion-only otherwise). + return track(detections, std::vector{}, frame); +} + + +std::vector> +BoTSORT::track(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame) { PROFILE_FUNCTION(); + + // Validate parallel-array contract. Empty features is allowed and means + // "fall back to internal extraction or motion-only". + if (!features.empty() && features.size() != detections.size()) + { + throw std::invalid_argument( + "BoTSORT::track: features.size() must equal " + "detections.size() when features are supplied"); + } + + // Appearance-aware association is enabled when either (a) the user has + // supplied externally-computed embeddings, or (b) the legacy internal + // ReID path is active. The TrackerParams::reid_enabled flag retains its + // original meaning: it gates internal extraction only. + const bool external_features = !features.empty(); + const bool use_appearance = external_features || _reid_enabled; + ////////////////// CREATE TRACK OBJECT FOR ALL THE DETECTIONS ////////////////// // For all detections, extract features, create tracks and classify on the segregate of confidence _frame_id++; @@ -113,9 +149,10 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) if (!detections.empty()) { - for (Detection &detection: - const_cast &>(detections)) + for (size_t det_idx = 0; det_idx < detections.size(); ++det_idx) { + Detection &detection = + const_cast(detections[det_idx]); detection.bbox_tlwh.x = std::max(0.0f, detection.bbox_tlwh.x); detection.bbox_tlwh.y = std::max(0.0f, detection.bbox_tlwh.y); detection.bbox_tlwh.width = @@ -132,7 +169,15 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) if (detection.confidence > _track_low_thresh) { - if (_reid_enabled) + if (external_features) + { + // Use the caller-supplied embedding verbatim — no host + // round-trip through cv::Mat, no internal model needed. + tracklet = std::make_shared( + tlwh, detection.confidence, detection.class_id, + features[det_idx]); + } + else if (_reid_enabled) { FeatureVector embedding = _extract_features(frame, detection.bbox_tlwh); @@ -197,13 +242,14 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) fuse_score(iou_dists, detections_high_conf);// Fuse the score with IoU distance - if (_reid_enabled) + if (use_appearance) { - // If re-ID is enabled, find the embedding distance between all tracked tracks and high confidence detections + // If appearance-aware association is enabled (either via internal + // ReID or externally-supplied embeddings), find the embedding + // distance between all tracked tracks and high confidence detections std::tie(raw_emd_dist, emd_dist_mask_1st_association) = embedding_distance(tracks_pool, detections_high_conf, - _appearance_thresh, - _reid_model->get_distance_metric()); + _appearance_thresh, _distance_metric); fuse_motion(*_kalman_filter, raw_emd_dist, tracks_pool, detections_high_conf, _lambda);// Fuse the motion with embedding distance @@ -321,14 +367,13 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) fuse_score(iou_dists_unconfirmed, unmatched_detections_after_1st_association); - if (_reid_enabled) + if (use_appearance) { // Find embedding distance between unconfirmed tracks and high confidence detections left after the first association std::tie(raw_emd_dist_unconfirmed, emd_dist_mask_unconfirmed) = embedding_distance(unconfirmed_tracks, unmatched_detections_after_1st_association, - _appearance_thresh, - _reid_model->get_distance_metric()); + _appearance_thresh, _distance_metric); fuse_motion(*_kalman_filter, raw_emd_dist_unconfirmed, unconfirmed_tracks, unmatched_detections_after_1st_association, _lambda); From cce78bdae68c2c4e3be19b31cdeccce03e4caf60 Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Thu, 14 May 2026 18:56:18 +0300 Subject: [PATCH 2/9] feat(BoTSORT): runtime setter for gmc_enabled Adds BoTSORT::set_gmc_enabled(bool) so the host pipeline can toggle camera motion compensation between successive track() calls. The existing constructor-time gmc_config still controls whether the GMC algorithm is built at all; this setter only enables/disables the per-frame apply step. Motivation: TrackerModule's adaptive-CMC gate skips CMC on stable broadcast scenes (no panning/zooming) to recover the v1.7 Sprint 4 tracker p99 budget (7.4 ms -> <5 ms target). The host detects stability from cross-frame track centroid drift; this setter is the actuator. The set_gmc_enabled(true) call is a no-op when _gmc_algo is null (GMC was never configured at construction), so callers never need to inspect the construction state to keep the call safe. --- botsort/include/BoTSORT.h | 20 ++++++++++++++++++++ botsort/src/BoTSORT.cpp | 13 +++++++++++++ 2 files changed, 33 insertions(+) diff --git a/botsort/include/BoTSORT.h b/botsort/include/BoTSORT.h index cd29cf5..91e79ca 100755 --- a/botsort/include/BoTSORT.h +++ b/botsort/include/BoTSORT.h @@ -62,6 +62,26 @@ class BoTSORT const std::vector &features, const cv::Mat &frame); + /** + * @brief Toggle GMC (Global Motion Compensation) at runtime. + * + * Callers can flip the per-frame CMC step on or off between successive + * calls to @ref track. Setting @p enabled=true is a no-op when the GMC + * algorithm was never constructed (i.e. the tracker was built without a + * GMC config), so the call is always safe. + * + * Intended use: adaptive skipping of CMC on stable broadcast scenes — + * see TrackerModule's adaptive-CMC gate. The next @ref track call + * honours the new state immediately; previously-applied homographies + * stay reflected in the existing Kalman state. + */ + void set_gmc_enabled(bool enabled) noexcept; + + /// Current state of the GMC gate. Useful for the host pipeline's + /// metrics / introspection paths. + [[nodiscard]] bool gmc_enabled() const noexcept { return _gmc_enabled; } + + private: /** * @brief Extract visual features from the given frame and bounding box diff --git a/botsort/src/BoTSORT.cpp b/botsort/src/BoTSORT.cpp index 2059750..2d032a4 100755 --- a/botsort/src/BoTSORT.cpp +++ b/botsort/src/BoTSORT.cpp @@ -488,6 +488,19 @@ BoTSORT::track(const std::vector &detections, } +void BoTSORT::set_gmc_enabled(bool enabled) noexcept +{ + // Guard against re-enabling GMC when no algorithm was constructed — + // _gmc_algo->apply() would crash. Construction-time absence of GMC + // (gmc_config empty) means the caller cannot opt back in mid-run. + if (enabled && !_gmc_algo) + { + return; + } + _gmc_enabled = enabled; +} + + FeatureVector BoTSORT::_extract_features(const cv::Mat &frame, const cv::Rect_ &bbox_tlwh) { From 06e6ab7d68d92b81b0b1d4a8a454f116e0d57c77 Mon Sep 17 00:00:00 2001 From: eng-EslamEzzat Date: Thu, 21 May 2026 01:11:03 +0300 Subject: [PATCH 3/9] feat(BoTSORT): add track() overload accepting precomputed homography For host pipelines that run a single shared GMC pass upstream and dispatch detections across multiple BoTSORT instances per frame (e.g. OpticXI's PerClassTrackerBank). When the caller supplies a HomographyMatrix, the internal _gmc_algo->apply() pass is skipped and the caller-supplied matrix is applied via Track::multi_gmc to both tracked + unconfirmed tracks. The existing two overloads stay unchanged. --- botsort/include/BoTSORT.h | 44 ++++++++++++++++++++++++++++++++++++++- botsort/src/BoTSORT.cpp | 31 +++++++++++++++++++++++++-- 2 files changed, 72 insertions(+), 3 deletions(-) diff --git a/botsort/include/BoTSORT.h b/botsort/include/BoTSORT.h index 91e79ca..182bf10 100755 --- a/botsort/include/BoTSORT.h +++ b/botsort/include/BoTSORT.h @@ -62,6 +62,33 @@ class BoTSORT const std::vector &features, const cv::Mat &frame); + /** + * @brief Track using pre-computed embeddings AND a caller-supplied + * camera-motion homography. + * + * Identical to the (detections, features, frame) overload, but the + * internal GMC pass is bypassed and the supplied @p H is applied to + * every confirmed/lost track instead. Designed for use by hosts that + * run a single shared GMC pass upstream and dispatch detections to + * multiple BoTSORT instances per frame (e.g. PerClassTrackerBank). + * + * @param detections Detections in the frame. + * @param features Pre-computed embeddings parallel to @p detections. + * Pass an empty vector for motion-only association. + * @param frame Frame; used for bbox clipping. GMC is NOT computed + * from this frame — @p H is used directly. + * @param H Pre-computed homography mapping the prior frame's + * camera plane into the current frame's. Pass an + * identity matrix to short-circuit motion application + * (equivalent to set_gmc_enabled(false) for one frame). + */ + std::vector> + track(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame, + const HomographyMatrix &H); + + /** * @brief Toggle GMC (Global Motion Compensation) at runtime. * @@ -83,9 +110,24 @@ class BoTSORT private: + /** + * @brief Shared implementation for the (detections, features, frame) + * and (detections, features, frame, H) overloads. + * + * When @p precomputed_H is non-null, the internal GMC pass is skipped + * and the supplied homography is applied to both the tracks pool and + * the unconfirmed tracks. When @p precomputed_H is null, the existing + * GMC behaviour applies (gated by @ref _gmc_enabled). + */ + std::vector> + _track_impl(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame, + const HomographyMatrix *precomputed_H); + /** * @brief Extract visual features from the given frame and bounding box - * + * * @param frame Input frame * @param bbox_tlwh Bounding box (top, left, width, height) * @return FeatureVector Extracted visual features diff --git a/botsort/src/BoTSORT.cpp b/botsort/src/BoTSORT.cpp index 2d032a4..ddefaee 100755 --- a/botsort/src/BoTSORT.cpp +++ b/botsort/src/BoTSORT.cpp @@ -119,6 +119,25 @@ std::vector> BoTSORT::track(const std::vector &detections, const std::vector &features, const cv::Mat &frame) +{ + return _track_impl(detections, features, frame, nullptr); +} + + +std::vector> +BoTSORT::track(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame, const HomographyMatrix &H) +{ + return _track_impl(detections, features, frame, &H); +} + + +std::vector> +BoTSORT::_track_impl(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame, + const HomographyMatrix *precomputed_H) { PROFILE_FUNCTION(); @@ -221,8 +240,16 @@ BoTSORT::track(const std::vector &detections, // Predict the location of the tracks with KF (even for lost tracks) Track::multi_predict(tracks_pool, *_kalman_filter); - // Estimate camera motion and apply camera motion compensation - if (_gmc_enabled) + // Apply camera motion compensation. When the caller supplied a homography + // (e.g. one upstream GMC pass shared by multiple BoTSORT instances), use + // it verbatim and skip the internal _gmc_algo->apply() call entirely. + // Otherwise honour _gmc_enabled and compute it ourselves. + if (precomputed_H != nullptr) + { + Track::multi_gmc(tracks_pool, *precomputed_H); + Track::multi_gmc(unconfirmed_tracks, *precomputed_H); + } + else if (_gmc_enabled) { HomographyMatrix H = _gmc_algo->apply(frame, detections); Track::multi_gmc(tracks_pool, H); From ef2e688d50163dc494f6f77eed57dbe755f65b6c Mon Sep 17 00:00:00 2001 From: eng-EslamEzzat Date: Sun, 24 May 2026 18:19:32 +0300 Subject: [PATCH 4/9] =?UTF-8?q?feat:=20add=20PitchKalmanFilter=20=E2=80=94?= =?UTF-8?q?=204-D=20metre-space=20constant-velocity=20KF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit For OpticXI Phase E.2 pitch-plane Kalman state. Runs in parallel with the existing 8-D pixel KalmanFilter; the host decides per-frame which gate to use. Non-finite measurement guard returns state unchanged. --- botsort/include/PitchKalmanFilter.h | 75 ++++++++++++++++++++ botsort/src/PitchKalmanFilter.cpp | 105 ++++++++++++++++++++++++++++ 2 files changed, 180 insertions(+) create mode 100644 botsort/include/PitchKalmanFilter.h create mode 100644 botsort/src/PitchKalmanFilter.cpp diff --git a/botsort/include/PitchKalmanFilter.h b/botsort/include/PitchKalmanFilter.h new file mode 100644 index 0000000..3533dd6 --- /dev/null +++ b/botsort/include/PitchKalmanFilter.h @@ -0,0 +1,75 @@ +#pragma once + +/// @file PitchKalmanFilter.h +/// @brief 4-D metre-space Kalman filter for camera-pan-invariant tracking. +/// +/// Phase E.2 of the OpticXI tracking pipeline. Runs in parallel with the +/// existing 8-D pixel KalmanFilter; the host bank decides per-frame which +/// gate to use based on homography validity. State is foot-point only: +/// width / height stay in pixels (perspective foreshortening makes +/// width-in-metres physically poorly defined). Constant-velocity model; +/// process / measurement noise scaled by metre-space standard deviations. + +#include +#include + +#include + +namespace bot_kalman { + +inline constexpr int PITCH_KF_STATE_DIM = 4; +inline constexpr int PITCH_KF_MEAS_DIM = 2; + +using PKFStateVec = Eigen::Matrix; +using PKFStateMat = Eigen::Matrix; +using PKFMeasVec = Eigen::Matrix; +using PKFMeasMat = Eigen::Matrix; + +using PKFDataState = std::pair; +using PKFDataMeas = std::pair; + +class PitchKalmanFilter { +public: + /// Chi-squared 95th percentile, 2 DoF (foot-point measurement). Used + /// as the Mahalanobis gating threshold. + static constexpr float chi2inv95 = 5.9915F; + + /// @param dt Frame interval in seconds (1 / fps). + /// @param std_weight_pos Position standard deviation, metres. + /// @param std_weight_vel Velocity standard deviation, m/s. + PitchKalmanFilter(float dt, float std_weight_pos, float std_weight_vel); + + /// Initialize from a measurement; velocity starts at zero. + PKFDataState init(const PKFMeasVec& measurement) const; + + /// Constant-velocity predict step (in place). + void predict(PKFStateVec& mean, PKFStateMat& covariance) const; + + /// Project (mean, covariance) to measurement space. + PKFDataMeas project(const PKFStateVec& mean, + const PKFStateMat& covariance) const; + + /// Standard Kalman update step. + /// If `measurement` contains any non-finite value, returns + /// (mean, covariance) unchanged. + PKFDataState update(const PKFStateVec& mean, + const PKFStateMat& covariance, + const PKFMeasVec& measurement) const; + + /// Squared Mahalanobis distance for each candidate measurement. + Eigen::Matrix gating_distance( + const PKFStateVec& mean, + const PKFStateMat& covariance, + const std::vector& measurements) const; + +private: + void _init_matrices(float dt); + + float _dt; + float _std_weight_position_m; + float _std_weight_velocity_m; + PKFStateMat _state_transition_matrix; + Eigen::Matrix _measurement_matrix; +}; + +} // namespace bot_kalman diff --git a/botsort/src/PitchKalmanFilter.cpp b/botsort/src/PitchKalmanFilter.cpp new file mode 100644 index 0000000..61d1b9f --- /dev/null +++ b/botsort/src/PitchKalmanFilter.cpp @@ -0,0 +1,105 @@ +#include "PitchKalmanFilter.h" + +#include +#include + +namespace bot_kalman { + +PitchKalmanFilter::PitchKalmanFilter(float dt, + float std_weight_pos, + float std_weight_vel) + : _dt(std::max(dt, 1e-3F)) + , _std_weight_position_m(std_weight_pos) + , _std_weight_velocity_m(std_weight_vel) +{ + _init_matrices(_dt); +} + +void PitchKalmanFilter::_init_matrices(float dt) { + _measurement_matrix.setZero(); + _measurement_matrix(0, 0) = 1.0F; // measure x + _measurement_matrix(1, 1) = 1.0F; // measure y + + _state_transition_matrix.setIdentity(); + _state_transition_matrix(0, 2) = dt; // x += vx * dt + _state_transition_matrix(1, 3) = dt; // y += vy * dt +} + +PKFDataState PitchKalmanFilter::init(const PKFMeasVec& measurement) const { + PKFStateVec mean = PKFStateVec::Zero(); + mean(0) = measurement(0); + mean(1) = measurement(1); + + PKFStateVec std_dev; + std_dev(0) = 2.0F * _std_weight_position_m; + std_dev(1) = 2.0F * _std_weight_position_m; + std_dev(2) = 10.0F * _std_weight_velocity_m; + std_dev(3) = 10.0F * _std_weight_velocity_m; + + PKFStateMat covariance = std_dev.array().square().matrix().asDiagonal(); + return {mean, covariance}; +} + +void PitchKalmanFilter::predict(PKFStateVec& mean, + PKFStateMat& covariance) const { + PKFStateVec std_dev; + std_dev(0) = _std_weight_position_m; + std_dev(1) = _std_weight_position_m; + std_dev(2) = _std_weight_velocity_m; + std_dev(3) = _std_weight_velocity_m; + PKFStateMat motion_cov = std_dev.array().square().matrix().asDiagonal(); + + mean = _state_transition_matrix * mean; + covariance = _state_transition_matrix * covariance * + _state_transition_matrix.transpose() + motion_cov; +} + +PKFDataMeas PitchKalmanFilter::project(const PKFStateVec& mean, + const PKFStateMat& covariance) const { + PKFMeasVec innovation_std; + innovation_std << _std_weight_position_m, _std_weight_position_m; + PKFMeasMat innovation_cov = innovation_std.array().square().matrix().asDiagonal(); + + PKFMeasVec projected_mean = _measurement_matrix * mean; + PKFMeasMat projected_cov = _measurement_matrix * covariance * + _measurement_matrix.transpose() + innovation_cov; + return {projected_mean, projected_cov}; +} + +PKFDataState PitchKalmanFilter::update(const PKFStateVec& mean, + const PKFStateMat& covariance, + const PKFMeasVec& measurement) const { + // Non-finite-measurement guard (design §4 edge cases). + if (!measurement.allFinite()) { + return {mean, covariance}; + } + auto [proj_mean, proj_cov] = project(mean, covariance); + + Eigen::Matrix B = + (covariance * _measurement_matrix.transpose()).transpose(); + Eigen::Matrix gain = + proj_cov.llt().solve(B).transpose(); + PKFMeasVec innovation = measurement - proj_mean; + + PKFStateVec updated_mean = mean + gain * innovation; + PKFStateMat updated_cov = covariance - gain * proj_cov * gain.transpose(); + return {updated_mean, updated_cov}; +} + +Eigen::Matrix PitchKalmanFilter::gating_distance( + const PKFStateVec& mean, + const PKFStateMat& covariance, + const std::vector& measurements) const { + + auto [proj_mean, proj_cov] = project(mean, covariance); + Eigen::LLT llt(proj_cov); + Eigen::Matrix dists(measurements.size()); + for (Eigen::Index i = 0; i < static_cast(measurements.size()); ++i) { + Eigen::VectorXf diff = measurements[i] - proj_mean; + Eigen::VectorXf y = llt.matrixL().solve(diff); + dists(i) = y.squaredNorm(); + } + return dists; +} + +} // namespace bot_kalman From b16536ec3393374272d5b5bdfb5273cf7e391261 Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Sun, 24 May 2026 18:36:01 +0300 Subject: [PATCH 5/9] =?UTF-8?q?feat:=20STrack=20=E2=80=94=20add=20metre-sp?= =?UTF-8?q?ace=20Kalman=20state=20+=20lifecycle=20methods?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit For OpticXI Phase E.2 pitch-plane Kalman. Adds _pitch_mean (4-D), _pitch_covariance (4x4), _pitch_kf_initialized (bool); public activate_pitch / predict_pitch / update_pitch / pitch_kf_initialized / pitch_mean / pitch_covariance. State lazily activates on the first matched detection where the host's homography is valid. --- botsort/include/track.h | 39 +++++++++++++++++++++++++++++++++++++++ botsort/src/track.cpp | 28 ++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+) diff --git a/botsort/include/track.h b/botsort/include/track.h index 2db131a..5f2af9a 100644 --- a/botsort/include/track.h +++ b/botsort/include/track.h @@ -5,8 +5,10 @@ #include "KalmanFilter.h" #include "KalmanFilterAccBased.h" +#include "PitchKalmanFilter.h" using KalmanFilter = bot_kalman::KalmanFilter; +using PitchKalmanFilter = bot_kalman::PitchKalmanFilter; enum TrackState { @@ -144,6 +146,36 @@ class Track void update(KalmanFilter &kalman_filter, Track &new_track, uint32_t frame_id); + /// Phase E.2: activate the metre-space Kalman from a pitch-projected + /// foot-point measurement. No-op if the metre state was already + /// initialized (use `update_pitch` instead). + void activate_pitch(PitchKalmanFilter& pitch_kf, + const bot_kalman::PKFMeasVec& measurement); + + /// Phase E.2: constant-velocity predict on the metre state. Caller + /// must check `pitch_kf_initialized()` first. + void predict_pitch(const PitchKalmanFilter& pitch_kf); + + /// Phase E.2: Kalman update on the metre state from a pitch-projected + /// foot-point measurement. Lazily activates if not yet initialized. + void update_pitch(PitchKalmanFilter& pitch_kf, + const bot_kalman::PKFMeasVec& measurement); + + /// Phase E.2: read-only accessor for the gate logic in BoTSORT. + [[nodiscard]] bool pitch_kf_initialized() const noexcept { + return _pitch_kf_initialized; + } + + /// Phase E.2: read-only mean (state vector) — used by the metre gate. + [[nodiscard]] const bot_kalman::PKFStateVec& pitch_mean() const noexcept { + return _pitch_mean; + } + + /// Phase E.2: read-only covariance — used by the metre gate. + [[nodiscard]] const bot_kalman::PKFStateMat& pitch_covariance() const noexcept { + return _pitch_covariance; + } + private: /** * @brief Updates visual feature vector and feature history @@ -204,4 +236,11 @@ class Track int _feat_history_size; std::deque> _feat_history; + + // Phase E.2 — metre-space Kalman state (parallel to mean / covariance). + // Lazily initialized: stays default-constructed until the first matched + // detection on a frame where the host's homography is valid. + bot_kalman::PKFStateVec _pitch_mean = bot_kalman::PKFStateVec::Zero(); + bot_kalman::PKFStateMat _pitch_covariance = bot_kalman::PKFStateMat::Zero(); + bool _pitch_kf_initialized = false; }; \ No newline at end of file diff --git a/botsort/src/track.cpp b/botsort/src/track.cpp index 5e11cef..aed69c4 100644 --- a/botsort/src/track.cpp +++ b/botsort/src/track.cpp @@ -265,4 +265,32 @@ void Track::_update_class_id(uint8_t class_id, float score) _class_hist.emplace_back(class_id, score); _class_id = class_id; } +} + +// ─── Phase E.2 — Metre-space Kalman lifecycle ──────────────────────────────── + +void Track::activate_pitch(PitchKalmanFilter& pitch_kf, + const bot_kalman::PKFMeasVec& measurement) { + auto [mean, cov] = pitch_kf.init(measurement); + _pitch_mean = mean; + _pitch_covariance = cov; + _pitch_kf_initialized = true; +} + +void Track::predict_pitch(const PitchKalmanFilter& pitch_kf) { + if (!_pitch_kf_initialized) { + return; + } + pitch_kf.predict(_pitch_mean, _pitch_covariance); +} + +void Track::update_pitch(PitchKalmanFilter& pitch_kf, + const bot_kalman::PKFMeasVec& measurement) { + if (!_pitch_kf_initialized) { + activate_pitch(pitch_kf, measurement); + return; + } + auto [mean, cov] = pitch_kf.update(_pitch_mean, _pitch_covariance, measurement); + _pitch_mean = mean; + _pitch_covariance = cov; } \ No newline at end of file From 6b317f6defcaea467c409b4e3cb9355a14cdd84c Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Sun, 24 May 2026 18:43:40 +0300 Subject: [PATCH 6/9] fix(STrack): activate_pitch docstring + const-correctness on pitch_kf params Code-review fix on E.2.2: - activate_pitch docstring claimed 'no-op if initialized', but the implementation unconditionally re-inits (legitimate re-activate after long occlusion). Docstring updated to match behavior; callers wanting seed-if-empty should use update_pitch (which lazy-activates). - activate_pitch and update_pitch took non-const PitchKalmanFilter& even though PitchKalmanFilter::init/predict/update are all const. Changed to const PitchKalmanFilter& (matches predict_pitch). - Renamed structured-binding locals from mean/cov to pkf_mean/pkf_cov to avoid shadowing Track's public 'mean' member. --- botsort/include/track.h | 12 +++++++----- botsort/src/track.cpp | 16 ++++++++-------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/botsort/include/track.h b/botsort/include/track.h index 5f2af9a..b7649bb 100644 --- a/botsort/include/track.h +++ b/botsort/include/track.h @@ -146,10 +146,12 @@ class Track void update(KalmanFilter &kalman_filter, Track &new_track, uint32_t frame_id); - /// Phase E.2: activate the metre-space Kalman from a pitch-projected - /// foot-point measurement. No-op if the metre state was already - /// initialized (use `update_pitch` instead). - void activate_pitch(PitchKalmanFilter& pitch_kf, + /// Phase E.2: initialize (or re-initialize) the metre-space Kalman + /// from a pitch-projected foot-point measurement. Always overwrites + /// any existing metre state and marks the track as metre-initialized; + /// callers who only want to seed when empty should use `update_pitch`, + /// which lazy-activates on first use. + void activate_pitch(const PitchKalmanFilter& pitch_kf, const bot_kalman::PKFMeasVec& measurement); /// Phase E.2: constant-velocity predict on the metre state. Caller @@ -158,7 +160,7 @@ class Track /// Phase E.2: Kalman update on the metre state from a pitch-projected /// foot-point measurement. Lazily activates if not yet initialized. - void update_pitch(PitchKalmanFilter& pitch_kf, + void update_pitch(const PitchKalmanFilter& pitch_kf, const bot_kalman::PKFMeasVec& measurement); /// Phase E.2: read-only accessor for the gate logic in BoTSORT. diff --git a/botsort/src/track.cpp b/botsort/src/track.cpp index aed69c4..2706767 100644 --- a/botsort/src/track.cpp +++ b/botsort/src/track.cpp @@ -269,11 +269,11 @@ void Track::_update_class_id(uint8_t class_id, float score) // ─── Phase E.2 — Metre-space Kalman lifecycle ──────────────────────────────── -void Track::activate_pitch(PitchKalmanFilter& pitch_kf, +void Track::activate_pitch(const PitchKalmanFilter& pitch_kf, const bot_kalman::PKFMeasVec& measurement) { - auto [mean, cov] = pitch_kf.init(measurement); - _pitch_mean = mean; - _pitch_covariance = cov; + auto [pkf_mean, pkf_cov] = pitch_kf.init(measurement); + _pitch_mean = pkf_mean; + _pitch_covariance = pkf_cov; _pitch_kf_initialized = true; } @@ -284,13 +284,13 @@ void Track::predict_pitch(const PitchKalmanFilter& pitch_kf) { pitch_kf.predict(_pitch_mean, _pitch_covariance); } -void Track::update_pitch(PitchKalmanFilter& pitch_kf, +void Track::update_pitch(const PitchKalmanFilter& pitch_kf, const bot_kalman::PKFMeasVec& measurement) { if (!_pitch_kf_initialized) { activate_pitch(pitch_kf, measurement); return; } - auto [mean, cov] = pitch_kf.update(_pitch_mean, _pitch_covariance, measurement); - _pitch_mean = mean; - _pitch_covariance = cov; + auto [pkf_mean, pkf_cov] = pitch_kf.update(_pitch_mean, _pitch_covariance, measurement); + _pitch_mean = pkf_mean; + _pitch_covariance = pkf_cov; } \ No newline at end of file From 91fd9e41e71eb952827ec5342df4e6d3c7b8f7f3 Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Sun, 24 May 2026 18:53:58 +0300 Subject: [PATCH 7/9] =?UTF-8?q?feat:=20BoTSORT=20=E2=80=94=20dual-KF=20wir?= =?UTF-8?q?ing=20+=20hybrid=20metre/pixel=20Mahalanobis=20gate?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit For OpticXI Phase E.2. Adds: - ctor param pitch_kalman_enabled (default false -> bit-identical to pre-E.2) - 4th track() overload taking per-detection optional metre foot-points - predict_pitch pass for tracks with initialized metre state - new fuse_motion_pitch helper (additive Mahalanobis tightening) - update_pitch / activate_pitch on matched detections that carry a metre measurement --- botsort/include/BoTSORT.h | 42 +++++++- botsort/include/matching.h | 35 +++++++ botsort/src/BoTSORT.cpp | 196 ++++++++++++++++++++++++++++++++++++- botsort/src/matching.cpp | 59 +++++++++++ 4 files changed, 328 insertions(+), 4 deletions(-) diff --git a/botsort/include/BoTSORT.h b/botsort/include/BoTSORT.h index 182bf10..6be1f79 100755 --- a/botsort/include/BoTSORT.h +++ b/botsort/include/BoTSORT.h @@ -1,10 +1,13 @@ #pragma once +#include #include #include +#include #include "GlobalMotionCompensation.h" #include "GmcParams.h" +#include "PitchKalmanFilter.h" #include "ReID.h" #include "ReIDParams.h" #include "TrackerParams.h" @@ -16,10 +19,19 @@ using Config = std::variant; class BoTSORT { public: + /// Phase E.2 (OpticXI): optional metre-space Kalman state per track. When + /// pitch_kalman_enabled = true, callers may pass per-detection metre + /// foot-point measurements via the new track() overload below; tracks + /// then run a parallel 4-D Kalman whose Mahalanobis distance tightens + /// the gate for that track-pair. When disabled (default) BoTSORT + /// behavior is bit-identical to pre-E.2. explicit BoTSORT(const Config &tracker_config, const Config &gmc_config = {}, const Config &reid_config = {}, - const std::string &reid_onnx_model_path = ""); + const std::string &reid_onnx_model_path = "", + bool pitch_kalman_enabled = false, + float pitch_kf_std_weight_position_m = 0.1F, + float pitch_kf_std_weight_velocity_m = 0.5F); ~BoTSORT() = default; @@ -89,6 +101,29 @@ class BoTSORT const HomographyMatrix &H); + /** + * @brief Phase E.2 (OpticXI) — track using embeddings + caller-supplied GMC + * homography + optional per-detection metre foot-points. + * + * Identical to the 3-arg (detections, features, frame, H) overload + * but also accepts a parallel std::optional metre measurement per + * detection. Entries where the optional is engaged feed the metre + * Kalman gate (an additive tightening on top of the pixel gate); + * std::nullopt entries leave the pixel gate's decision unchanged for + * that (track, detection) pair. An empty metre_measurements vector + * means "metre path off this frame" — equivalent to all-nullopt. + * + * @throws std::invalid_argument if metre_measurements is non-empty + * AND its size differs from detections.size(). + */ + std::vector> + track(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame, + const HomographyMatrix &H, + const std::vector>& metre_measurements); + + /** * @brief Toggle GMC (Global Motion Compensation) at runtime. * @@ -198,6 +233,11 @@ class BoTSORT std::vector> _lost_tracks; std::unique_ptr _kalman_filter; + // Phase E.2 — metre-space Kalman, optional (gated by _pitch_kalman_enabled). + bool _pitch_kalman_enabled{false}; + std::unique_ptr _pitch_kalman_filter; + // Stashed for the per-frame _track_impl pass; non-owning, cleared each frame. + const std::vector>* _frame_metre_measurements{nullptr}; std::unique_ptr _gmc_algo; std::unique_ptr _reid_model; }; \ No newline at end of file diff --git a/botsort/include/matching.h b/botsort/include/matching.h index 4529be2..aedbfb3 100644 --- a/botsort/include/matching.h +++ b/botsort/include/matching.h @@ -1,9 +1,12 @@ #pragma once #include +#include #include +#include #include "DataType.h" +#include "PitchKalmanFilter.h" #include "track.h" /** @@ -73,6 +76,38 @@ void fuse_motion(const KalmanFilter &KF, CostMatrix &cost_matrix, const std::vector> &detections, float lambda = 0.98F, bool only_position = false); +/** + * @brief Phase E.2 (OpticXI) — additive metre-space Mahalanobis gate. + * + * For each (track i, detection j) where TRACK has pitch_kf_initialized() + * AND DETECTION has a metre measurement (the optional at + * metre_measurements[j] is engaged), compute the metre Mahalanobis + * distance via @p pitch_kf and override cost_matrix(i, j) to +infinity + * if it exceeds PitchKalmanFilter::chi2inv95. Cells where either side + * lacks metre state are LEFT UNCHANGED, so the upstream pixel gate's + * decision stands. + * + * No-ops (returns early) when the input matrices are empty or when the + * metre_measurements vector size doesn't match detections.size() (caller + * contract violation — fail-safe to leave the cost matrix untouched). + * + * @param pitch_kf The metre-space KF (caller owns). + * @param cost_matrix In/out. Same shape as tracks × detections. + * @param tracks The tracks rows correspond to. + * @param detections The detections cols correspond to (only the + * size is checked here; the measurement values + * come from metre_measurements). + * @param metre_measurements Parallel to detections (size must equal + * detections.size()). std::nullopt entries + * are skipped (cell left unchanged). + */ +void fuse_motion_pitch( + const PitchKalmanFilter &pitch_kf, + CostMatrix &cost_matrix, + const std::vector> &tracks, + const std::vector> &detections, + const std::vector> &metre_measurements); + /** * @brief Fuse IoU distance with embedding distance keeping the mask in mind * diff --git a/botsort/src/BoTSORT.cpp b/botsort/src/BoTSORT.cpp index ddefaee..edd383f 100755 --- a/botsort/src/BoTSORT.cpp +++ b/botsort/src/BoTSORT.cpp @@ -50,7 +50,10 @@ T fetch_config(const Config &config, BoTSORT::BoTSORT(const Config &tracker_config, const Config &gmc_config, const Config &reid_config, - const std::string &reid_onnx_model_path) + const std::string &reid_onnx_model_path, + bool pitch_kalman_enabled, + float pitch_kf_std_weight_position_m, + float pitch_kf_std_weight_velocity_m) { auto tracker_params = fetch_config( tracker_config, TrackerParams::load_config); @@ -63,6 +66,17 @@ BoTSORT::BoTSORT(const Config &tracker_config, _kalman_filter = std::make_unique( static_cast(1.0 / _frame_rate)); + // Phase E.2 — metre-space Kalman, optional. When disabled the rest of + // _track_impl bypasses every metre code path (see pitch_pass_active), + // making behavior bit-identical to pre-E.2. + _pitch_kalman_enabled = pitch_kalman_enabled; + if (_pitch_kalman_enabled) { + const float dt = 1.0F / static_cast(_frame_rate); + _pitch_kalman_filter = std::make_unique( + dt, pitch_kf_std_weight_position_m, + pitch_kf_std_weight_velocity_m); + } + // Re-ID module, load visual feature extractor here if (_reid_enabled && not_empty(reid_config) && @@ -133,6 +147,34 @@ BoTSORT::track(const std::vector &detections, } +std::vector> +BoTSORT::track(const std::vector &detections, + const std::vector &features, + const cv::Mat &frame, const HomographyMatrix &H, + const std::vector>& + metre_measurements) +{ + // Validate parallel-array contract. An empty metre_measurements vector + // means "metre path off this frame" and is always permitted; a non-empty + // vector must be 1:1 with detections (same indexing). + if (!metre_measurements.empty() && + metre_measurements.size() != detections.size()) + { + throw std::invalid_argument( + "BoTSORT::track — metre_measurements size mismatch with " + "detections"); + } + // Stash for the duration of this _track_impl call. We use a raw pointer + // because std::vector> isn't trivially copyable on + // the hot path; cleared immediately after so subsequent calls without + // metre measurements aren't accidentally affected. + _frame_metre_measurements = &metre_measurements; + auto out = _track_impl(detections, features, frame, &H); + _frame_metre_measurements = nullptr; + return out; +} + + std::vector> BoTSORT::_track_impl(const std::vector &detections, const std::vector &features, @@ -166,6 +208,19 @@ BoTSORT::_track_impl(const std::vector &detections, detections_low_conf.reserve(detections.size()), detections_high_conf.reserve(detections.size()); + // Phase E.2 — parallel metre-measurement subsets that match the + // high-conf / low-conf detection vectors index-for-index. We populate + // them in the same pass as the detection split so the column ordering + // stays in lock-step with the cost matrices later on. + const bool have_metre = (_frame_metre_measurements != nullptr) && + !_frame_metre_measurements->empty(); + std::vector> + metre_meas_high_conf, metre_meas_low_conf; + if (have_metre) { + metre_meas_high_conf.reserve(detections.size()); + metre_meas_low_conf.reserve(detections.size()); + } + if (!detections.empty()) { for (size_t det_idx = 0; det_idx < detections.size(); ++det_idx) @@ -209,9 +264,21 @@ BoTSORT::_track_impl(const std::vector &detections, tlwh, detection.confidence, detection.class_id); if (detection.confidence >= _track_high_thresh) + { detections_high_conf.push_back(tracklet); + if (have_metre) { + metre_meas_high_conf.push_back( + (*_frame_metre_measurements)[det_idx]); + } + } else + { detections_low_conf.push_back(tracklet); + if (have_metre) { + metre_meas_low_conf.push_back( + (*_frame_metre_measurements)[det_idx]); + } + } } } } @@ -240,6 +307,22 @@ BoTSORT::_track_impl(const std::vector &detections, // Predict the location of the tracks with KF (even for lost tracks) Track::multi_predict(tracks_pool, *_kalman_filter); + // Phase E.2 — predict metre-space state for tracks that have it initialized. + // Skipped entirely when pitch_kalman is disabled OR when this frame has no + // metre measurements (host's homography invalid). The bool is computed + // here once and reused by every subsequent metre code path so the + // disabled path is bit-identical to pre-E.2. + const bool pitch_pass_active = _pitch_kalman_enabled && + _pitch_kalman_filter && + have_metre; + if (pitch_pass_active) { + for (auto& track_ptr : tracks_pool) { + if (track_ptr->pitch_kf_initialized()) { + track_ptr->predict_pitch(*_pitch_kalman_filter); + } + } + } + // Apply camera motion compensation. When the caller supplied a homography // (e.g. one upstream GMC pass shared by multiple BoTSORT instances), use // it verbatim and skip the internal _gmc_algo->apply() call entirely. @@ -282,6 +365,21 @@ BoTSORT::_track_impl(const std::vector &detections, _lambda);// Fuse the motion with embedding distance } + if (pitch_pass_active) { + // Phase E.2 — additive metre Mahalanobis tightening on the cost + // matrices feeding the 1st association. We tighten BOTH the + // appearance-side matrix (only relevant when use_appearance) and the + // IoU matrix so the metre gate isn't bypassed for (track, det) pairs + // that the appearance branch never visits. + if (use_appearance) { + fuse_motion_pitch(*_pitch_kalman_filter, raw_emd_dist, + tracks_pool, detections_high_conf, + metre_meas_high_conf); + } + fuse_motion_pitch(*_pitch_kalman_filter, iou_dists, tracks_pool, + detections_high_conf, metre_meas_high_conf); + } + // Fuse the IoU distance and embedding distance to get the final distance matrix CostMatrix distances_first_association = fuse_iou_with_emb( iou_dists, raw_emd_dist, iou_dists_mask_1st_association, @@ -311,6 +409,16 @@ BoTSORT::_track_impl(const std::vector &detections, track->re_activate(*_kalman_filter, *detection, _frame_id, false); refind_tracks.push_back(track); } + + // Phase E.2 — keep metre-space state in lock-step with the pixel KF. + // update_pitch lazily activates on first use, so no explicit + // activate_pitch branch needed for re-activations either. + if (pitch_pass_active) { + const auto& maybe_metre = metre_meas_high_conf[match.second]; + if (maybe_metre.has_value()) { + track->update_pitch(*_pitch_kalman_filter, maybe_metre.value()); + } + } } ////////////////// First association, with high score detection boxes ////////////////// @@ -332,6 +440,16 @@ BoTSORT::_track_impl(const std::vector &detections, iou_dists_second = iou_distance(unmatched_tracks_after_1st_association, detections_low_conf); + if (pitch_pass_active) { + // Phase E.2 — tighten the 2nd-association IoU matrix with the metre + // gate. The 2nd association doesn't run fuse_motion at all (pixel KF + // gating is skipped here in the original code); metre gate is the + // only Mahalanobis check on this branch. + fuse_motion_pitch(*_pitch_kalman_filter, iou_dists_second, + unmatched_tracks_after_1st_association, + detections_low_conf, metre_meas_low_conf); + } + // Perform linear assignment on the distance matrix, LAPJV algorithm is used here AssociationData second_associations = linear_assignment(iou_dists_second, 0.5); @@ -357,6 +475,14 @@ BoTSORT::_track_impl(const std::vector &detections, track->re_activate(*_kalman_filter, *detection, _frame_id, false); refind_tracks.push_back(track); } + + // Phase E.2 — metre update for the 2nd-association branch. + if (pitch_pass_active) { + const auto& maybe_metre = metre_meas_low_conf[match.second]; + if (maybe_metre.has_value()) { + track->update_pitch(*_pitch_kalman_filter, maybe_metre.value()); + } + } } // The tracks that are not associated with any detection even after the second association are marked as lost @@ -377,11 +503,24 @@ BoTSORT::_track_impl(const std::vector &detections, ////////////////// Deal with unconfirmed tracks ////////////////// std::vector> unmatched_detections_after_1st_association; + // Phase E.2 — parallel metre vec for the unconfirmed pass; sourced from + // metre_meas_high_conf at the same indices used to build the detection + // subset above so column ordering matches the cost matrices. + std::vector> + metre_meas_unmatched_high_conf; + if (pitch_pass_active) { + metre_meas_unmatched_high_conf.reserve( + first_associations.unmatched_det_indices.size()); + } for (int detection_idx: first_associations.unmatched_det_indices) { const std::shared_ptr &detection = detections_high_conf[detection_idx]; unmatched_detections_after_1st_association.push_back(detection); + if (pitch_pass_active) { + metre_meas_unmatched_high_conf.push_back( + metre_meas_high_conf[detection_idx]); + } } //Find IoU distance between unconfirmed tracks and high confidence detections left after the first association @@ -406,6 +545,23 @@ BoTSORT::_track_impl(const std::vector &detections, unmatched_detections_after_1st_association, _lambda); } + if (pitch_pass_active) { + // Phase E.2 — additive metre Mahalanobis tightening on the + // unconfirmed-tracks association. Same rationale as the 1st + // association: tighten both the appearance (when present) and + // IoU matrices. + if (use_appearance) { + fuse_motion_pitch(*_pitch_kalman_filter, raw_emd_dist_unconfirmed, + unconfirmed_tracks, + unmatched_detections_after_1st_association, + metre_meas_unmatched_high_conf); + } + fuse_motion_pitch(*_pitch_kalman_filter, iou_dists_unconfirmed, + unconfirmed_tracks, + unmatched_detections_after_1st_association, + metre_meas_unmatched_high_conf); + } + // Fuse the IoU distance and the embedding distance CostMatrix distances_unconfirmed = fuse_iou_with_emb( iou_dists_unconfirmed, raw_emd_dist_unconfirmed, @@ -425,6 +581,15 @@ BoTSORT::_track_impl(const std::vector &detections, // and add the track to the activated tracks list track->update(*_kalman_filter, *detection, _frame_id); activated_tracks.push_back(track); + + // Phase E.2 — metre update for the unconfirmed-track branch. + if (pitch_pass_active) { + const auto& maybe_metre = + metre_meas_unmatched_high_conf[match.second]; + if (maybe_metre.has_value()) { + track->update_pitch(*_pitch_kalman_filter, maybe_metre.value()); + } + } } // All the unconfirmed tracks that are not associated with any detection are marked as removed @@ -442,21 +607,46 @@ BoTSORT::_track_impl(const std::vector &detections, ////////////////// Initialize new tracks ////////////////// std::vector> unmatched_high_conf_detections; + // Phase E.2 — parallel metre vec for newly-born tracks. + std::vector> + metre_meas_new_tracks; + if (pitch_pass_active) { + metre_meas_new_tracks.reserve( + unconfirmed_associations.unmatched_det_indices.size()); + } for (int detection_idx: unconfirmed_associations.unmatched_det_indices) { const std::shared_ptr &detection = unmatched_detections_after_1st_association[detection_idx]; unmatched_high_conf_detections.push_back(detection); + if (pitch_pass_active) { + metre_meas_new_tracks.push_back( + metre_meas_unmatched_high_conf[detection_idx]); + } } // Initialize new tracks for the high confidence detections left after all the associations - for (const std::shared_ptr &detection: - unmatched_high_conf_detections) + for (size_t new_idx = 0; + new_idx < unmatched_high_conf_detections.size(); ++new_idx) { + const std::shared_ptr &detection = + unmatched_high_conf_detections[new_idx]; if (detection->get_score() >= _new_track_thresh) { detection->activate(*_kalman_filter, _frame_id); activated_tracks.push_back(detection); + + // Phase E.2 — seed metre state on newborn tracks when the host + // provided a metre measurement. update_pitch lazy-activates with + // velocity = 0, so the second frame's predict gives a sensible + // prior even without an explicit activate_pitch call. + if (pitch_pass_active) { + const auto& maybe_metre = metre_meas_new_tracks[new_idx]; + if (maybe_metre.has_value()) { + detection->update_pitch(*_pitch_kalman_filter, + maybe_metre.value()); + } + } } } ////////////////// Initialize new tracks ////////////////// diff --git a/botsort/src/matching.cpp b/botsort/src/matching.cpp index dc69365..1a7a51f 100644 --- a/botsort/src/matching.cpp +++ b/botsort/src/matching.cpp @@ -1,5 +1,7 @@ #include "matching.h" +#include + #include "DataType.h" #include "utils.h" @@ -174,6 +176,63 @@ void fuse_motion(const KalmanFilter &KF, CostMatrix &cost_matrix, } } +void fuse_motion_pitch( + const PitchKalmanFilter &pitch_kf, + CostMatrix &cost_matrix, + const std::vector> &tracks, + const std::vector> &detections, + const std::vector> &metre_measurements) +{ + if (cost_matrix.rows() == 0 || cost_matrix.cols() == 0) + { + return; + } + if (tracks.empty() || detections.empty()) + { + return; + } + // Caller contract: metre_measurements is parallel to detections. If the + // sizes disagree we'd risk an out-of-bounds read; fail-safe to no-op so + // the upstream pixel gate's decision stands. + if (metre_measurements.size() != detections.size()) + { + return; + } + + constexpr float kGateThreshold = bot_kalman::PitchKalmanFilter::chi2inv95; + + for (Eigen::Index i = 0; + i < static_cast(tracks.size()); ++i) + { + if (!tracks[i]->pitch_kf_initialized()) + { + continue; + } + for (Eigen::Index j = 0; + j < static_cast(detections.size()); ++j) + { + if (!metre_measurements[j].has_value()) + { + continue; + } + // gating_distance takes a vector of candidates; we pass a single + // probe per (track, det) cell because the metre KF doesn't + // batch across track rows. + std::vector probe{ + metre_measurements[j].value()}; + auto dists = pitch_kf.gating_distance( + tracks[i]->pitch_mean(), + tracks[i]->pitch_covariance(), + probe); + if (dists(0, 0) > kGateThreshold) + { + cost_matrix(i, j) = std::numeric_limits::infinity(); + } + // Else leave the upstream pixel gate's decision unchanged. + } + } +} + CostMatrix fuse_iou_with_emb(CostMatrix &iou_dist, CostMatrix &emb_dist, const CostMatrix &iou_dists_mask, const CostMatrix &emb_dists_mask) From 324ed826f2544f8112d7179fe63b6bcc481eb78d Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Sun, 24 May 2026 19:02:25 +0300 Subject: [PATCH 8/9] fix(BoTSORT): exception-safe stash guard + O(N) Cholesky in fuse_motion_pitch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Code-review fixes on E.2.3: - _frame_metre_measurements is now reset by an RAII guard so _track_impl can throw without leaving the borrowed pointer dangling into a popped caller stack frame. - fuse_motion_pitch collects valid metre measurements once and calls PitchKalmanFilter::gating_distance once per track instead of once per (track, detection) cell. Per-frame Cholesky count drops from O(tracks x dets x passes) to O(tracks x passes) — ~25x for typical 22-player scenes. --- botsort/src/BoTSORT.cpp | 15 ++++++++---- botsort/src/matching.cpp | 49 ++++++++++++++++++++++++++-------------- 2 files changed, 42 insertions(+), 22 deletions(-) diff --git a/botsort/src/BoTSORT.cpp b/botsort/src/BoTSORT.cpp index edd383f..7073944 100755 --- a/botsort/src/BoTSORT.cpp +++ b/botsort/src/BoTSORT.cpp @@ -166,12 +166,17 @@ BoTSORT::track(const std::vector &detections, } // Stash for the duration of this _track_impl call. We use a raw pointer // because std::vector> isn't trivially copyable on - // the hot path; cleared immediately after so subsequent calls without - // metre measurements aren't accidentally affected. + // the hot path; an RAII guard clears it on ALL exit paths (including + // exceptions from _track_impl) so subsequent track() calls that never + // set the pointer don't dereference a popped stack frame. + struct MetreStashGuard + { + BoTSORT *self; + ~MetreStashGuard() { self->_frame_metre_measurements = nullptr; } + }; _frame_metre_measurements = &metre_measurements; - auto out = _track_impl(detections, features, frame, &H); - _frame_metre_measurements = nullptr; - return out; + MetreStashGuard guard{this}; + return _track_impl(detections, features, frame, &H); } diff --git a/botsort/src/matching.cpp b/botsort/src/matching.cpp index 1a7a51f..8ea6a69 100644 --- a/botsort/src/matching.cpp +++ b/botsort/src/matching.cpp @@ -201,6 +201,28 @@ void fuse_motion_pitch( constexpr float kGateThreshold = bot_kalman::PitchKalmanFilter::chi2inv95; + // Collect the valid metre measurements ONCE (and remember their column + // indices), then call gating_distance once per track. This drops the + // Cholesky decomposition count from O(tracks x dets) to O(tracks), + // mirroring the batched pattern used by fuse_motion() above. + std::vector valid_meas; + std::vector valid_cols; + valid_meas.reserve(metre_measurements.size()); + valid_cols.reserve(metre_measurements.size()); + for (Eigen::Index j = 0; + j < static_cast(detections.size()); ++j) + { + if (metre_measurements[j].has_value()) + { + valid_meas.push_back(metre_measurements[j].value()); + valid_cols.push_back(j); + } + } + if (valid_meas.empty()) + { + return; + } + for (Eigen::Index i = 0; i < static_cast(tracks.size()); ++i) { @@ -208,25 +230,18 @@ void fuse_motion_pitch( { continue; } - for (Eigen::Index j = 0; - j < static_cast(detections.size()); ++j) + // ONE gating_distance call per track (one project() + Cholesky), + // batched across all valid metre measurements for this frame. + auto dists = pitch_kf.gating_distance( + tracks[i]->pitch_mean(), + tracks[i]->pitch_covariance(), + valid_meas); + for (Eigen::Index k = 0; k < dists.size(); ++k) { - if (!metre_measurements[j].has_value()) + if (dists(k) > kGateThreshold) { - continue; - } - // gating_distance takes a vector of candidates; we pass a single - // probe per (track, det) cell because the metre KF doesn't - // batch across track rows. - std::vector probe{ - metre_measurements[j].value()}; - auto dists = pitch_kf.gating_distance( - tracks[i]->pitch_mean(), - tracks[i]->pitch_covariance(), - probe); - if (dists(0, 0) > kGateThreshold) - { - cost_matrix(i, j) = std::numeric_limits::infinity(); + cost_matrix(i, valid_cols[k]) = + std::numeric_limits::infinity(); } // Else leave the upstream pixel gate's decision unchanged. } From 972ab3bc469df428cf5923b58758a7fcafe5e6c9 Mon Sep 17 00:00:00 2001 From: Ahmed Ramadan Date: Mon, 25 May 2026 17:47:13 +0300 Subject: [PATCH 9/9] feat(BoTSORT): expose frame_id() public getter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The host pipeline (PerClassTrackerBank) needs the current frame counter to compute per-track lost-duration (current_frame - bt->frame_id) for downstream consumers like the E.3 ROI module's recency gate. Pure additive — no behavior change. --- botsort/include/BoTSORT.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/botsort/include/BoTSORT.h b/botsort/include/BoTSORT.h index 6be1f79..47e3874 100755 --- a/botsort/include/BoTSORT.h +++ b/botsort/include/BoTSORT.h @@ -143,6 +143,12 @@ class BoTSORT /// metrics / introspection paths. [[nodiscard]] bool gmc_enabled() const noexcept { return _gmc_enabled; } + /// Current frame counter (monotonic, 1-based after the first track() call). + /// Exposed so the host pipeline can compute per-track lost-duration + /// (`frame_id() - track->frame_id`) for downstream consumers like the + /// E.3 ROI module that need to know how long a Lost track has been gone. + [[nodiscard]] unsigned int frame_id() const noexcept { return _frame_id; } + private: /**