Feat/e2 pitch kalman

botsort/include/BoTSORT.h

@@ -1,10 +1,13 @@
 #pragma once
 
+#include <optional>
 #include <string>
 #include <variant>
+#include <vector>
 
 #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<T, std::string, std::monostate>;
 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<TrackerParams> &tracker_config,
                      const Config<GMC_Params> &gmc_config = {},
                      const Config<ReIDParams> &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<std::shared_ptr<Track>> 
+     * @return std::vector<std::shared_ptr<Track>>
      */
     std::vector<std::shared_ptr<Track>>
     track(const std::vector<Detection> &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<std::shared_ptr<Track>>
+    track(const std::vector<Detection> &detections,
+          const std::vector<FeatureVector> &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<std::shared_ptr<Track>>
+    track(const std::vector<Detection> &detections,
+          const std::vector<FeatureVector> &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<std::shared_ptr<Track>>
+    track(const std::vector<Detection> &detections,
+          const std::vector<FeatureVector> &features,
+          const cv::Mat &frame,
+          const HomographyMatrix &H,
+          const std::vector<std::optional<bot_kalman::PKFMeasVec>>& 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<std::shared_ptr<Track>>
+    _track_impl(const std::vector<Detection> &detections,
+                const std::vector<FeatureVector> &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<std::shared_ptr<Track>> _lost_tracks;
 
     std::unique_ptr<KalmanFilter> _kalman_filter;
+    // Phase E.2 — metre-space Kalman, optional (gated by _pitch_kalman_enabled).
+    bool _pitch_kalman_enabled{false};
+    std::unique_ptr<PitchKalmanFilter> _pitch_kalman_filter;
+    // Stashed for the per-frame _track_impl pass; non-owning, cleared each frame.
+    const std::vector<std::optional<bot_kalman::PKFMeasVec>>* _frame_metre_measurements{nullptr};
     std::unique_ptr<GlobalMotionCompensation> _gmc_algo;
     std::unique_ptr<ReIDModel> _reid_model;
 };

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 <utility>
+#include <vector>
+
+#include <eigen3/Eigen/Dense>
+
+namespace bot_kalman {
+
+inline constexpr int PITCH_KF_STATE_DIM = 4;
+inline constexpr int PITCH_KF_MEAS_DIM  = 2;
+
+using PKFStateVec = Eigen::Matrix<float, PITCH_KF_STATE_DIM, 1>;
+using PKFStateMat = Eigen::Matrix<float, PITCH_KF_STATE_DIM, PITCH_KF_STATE_DIM>;
+using PKFMeasVec  = Eigen::Matrix<float, PITCH_KF_MEAS_DIM, 1>;
+using PKFMeasMat  = Eigen::Matrix<float, PITCH_KF_MEAS_DIM, PITCH_KF_MEAS_DIM>;
+
+using PKFDataState = std::pair<PKFStateVec, PKFStateMat>;
+using PKFDataMeas  = std::pair<PKFMeasVec,  PKFMeasMat>;
+
+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<float, 1, Eigen::Dynamic> gating_distance(
+        const PKFStateVec& mean,
+        const PKFStateMat& covariance,
+        const std::vector<PKFMeasVec>& 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<float, PITCH_KF_MEAS_DIM, PITCH_KF_STATE_DIM> _measurement_matrix;
+};
+
+}  // namespace bot_kalman

botsort/include/matching.h

@@ -1,9 +1,12 @@
 #pragma once
 
 #include <iostream>
+#include <optional>
 #include <tuple>
+#include <vector>
 
 #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<std::shared_ptr<Track>> &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<std::shared_ptr<Track>> &tracks,
+    const std::vector<std::shared_ptr<Track>> &detections,
+    const std::vector<std::optional<bot_kalman::PKFMeasVec>> &metre_measurements);
+
 /**
  * @brief Fuse IoU distance with embedding distance keeping the mask in mind
  * 

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<std::shared_ptr<FeatureVector>> _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;
 };