diff --git a/botsort/include/BoTSORT.h b/botsort/include/BoTSORT.h index 684eb20..47e3874 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,29 +19,156 @@ 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; /** * @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); + + + /** + * @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 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. + * + * 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; } + + /// 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: + /** + * @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 @@ -95,6 +225,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, @@ -105,6 +239,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/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/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/include/track.h b/botsort/include/track.h index 2db131a..b7649bb 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,38 @@ class Track void update(KalmanFilter &kalman_filter, Track &new_track, uint32_t frame_id); + /// 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 + /// 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(const 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 +238,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/BoTSORT.cpp b/botsort/src/BoTSORT.cpp index e6c84f1..7073944 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) && @@ -79,6 +93,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 +121,89 @@ 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) +{ + 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(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; 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; + MetreStashGuard guard{this}; + 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(); + + // 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++; @@ -111,11 +213,25 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) 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 (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 +248,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); @@ -145,9 +269,21 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) 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]); + } + } } } } @@ -176,8 +312,32 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) // 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) + // 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. + // 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); @@ -197,18 +357,34 @@ 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 } + 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, @@ -238,6 +414,16 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) 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 ////////////////// @@ -259,6 +445,16 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) 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); @@ -284,6 +480,14 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) 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 @@ -304,11 +508,24 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) ////////////////// 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 @@ -321,19 +538,35 @@ 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); } + 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, @@ -353,6 +586,15 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) // 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 @@ -370,21 +612,46 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) ////////////////// 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 ////////////////// @@ -443,6 +710,19 @@ BoTSORT::track(const std::vector &detections, const cv::Mat &frame) } +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) { 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 diff --git a/botsort/src/matching.cpp b/botsort/src/matching.cpp index dc69365..8ea6a69 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,78 @@ 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; + + // 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) + { + if (!tracks[i]->pitch_kf_initialized()) + { + continue; + } + // 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 (dists(k) > kGateThreshold) + { + cost_matrix(i, valid_cols[k]) = + 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) diff --git a/botsort/src/track.cpp b/botsort/src/track.cpp index 5e11cef..2706767 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(const PitchKalmanFilter& pitch_kf, + const bot_kalman::PKFMeasVec& measurement) { + auto [pkf_mean, pkf_cov] = pitch_kf.init(measurement); + _pitch_mean = pkf_mean; + _pitch_covariance = pkf_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(const PitchKalmanFilter& pitch_kf, + const bot_kalman::PKFMeasVec& measurement) { + if (!_pitch_kf_initialized) { + activate_pitch(pitch_kf, measurement); + return; + } + 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