first commit
This commit is contained in:
@@ -0,0 +1,87 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// kalman_box_filter.h —— 单目标 8 维恒定速度卡尔曼滤波器
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md §4（卡尔曼滤波算法细节）、§8.2（内部常量）
|
||||
// 状态向量 x[8] = [cx, cy, w, h, vx, vy, vw, vh]（§4.1）
|
||||
// 观测向量 z[4] = [cx, cy, w, h]（§4.1）
|
||||
// 时间步长 dt = 1 帧，与帧率无关（§1.3）
|
||||
//
|
||||
// 实现约束（§9.3）：矩阵规模极小（8×8 / 4×8 / 4×4），手写定长矩阵运算，
|
||||
// 全程 double 精度，不引入 Eigen/OpenCV 等线性代数库。
|
||||
// =============================================================================
|
||||
|
||||
#include <array>
|
||||
#include <optional>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
/// 轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）
|
||||
struct BoxCXCYWH {
|
||||
double cx = 0.0; // 框中心 x（像素）
|
||||
double cy = 0.0; // 框中心 y（像素）
|
||||
double w = 0.0; // 框宽（像素）
|
||||
double h = 0.0; // 框高（像素）
|
||||
};
|
||||
|
||||
/// 单目标恒定速度卡尔曼滤波器（§3.1 / §4）
|
||||
class KalmanBoxFilter {
|
||||
public:
|
||||
KalmanBoxFilter() = default;
|
||||
|
||||
/// 是否已用首次观测初始化（§3.1 initialized 字段）
|
||||
bool Initialized() const { return initialized_; }
|
||||
|
||||
/// 用首次观测 [cx, cy, w, h] 初始化（§4.3）：
|
||||
/// 状态速度置零；P = diag(2,2,2,2,100,100,100,100)。
|
||||
void Initiate(const BoxCXCYWH& z);
|
||||
|
||||
/// 预测推进一帧（§4.4）：未初始化返回 std::nullopt；
|
||||
/// 否则 x ← F·x，P ← F·P·Fᵀ + Q，返回状态前 4 维 [cx,cy,w,h] 副本。
|
||||
std::optional<BoxCXCYWH> Predict();
|
||||
|
||||
/// 观测更新（§4.5）：未初始化时直接返回；
|
||||
/// S = H·P·Hᵀ + R，K = P·Hᵀ·S⁻¹，x ← x + K(z−Hx)，P ← (I−KH)P。
|
||||
void Update(const BoxCXCYWH& z);
|
||||
|
||||
/// 状态向量只读访问（[cx, cy, w, h, vx, vy, vw, vh]，测试与排查用）
|
||||
const std::array<double, 8>& State() const { return x_; }
|
||||
|
||||
// ---- 内部常量（§4.2 / §8.2，经验值，针对 640×480 画面调优） ----
|
||||
static constexpr double kStdPos = 1.0 / 50.0; // STD_POS：位置噪声比例系数
|
||||
static constexpr double kStdVel = 1.0 / 200.0; // STD_VEL：速度噪声比例系数
|
||||
static constexpr double kIniPos = 2.0; // INI_POS：初始位置协方差
|
||||
static constexpr double kIniVel = 100.0; // INI_VEL：初始速度协方差
|
||||
static constexpr double kRScale = 2.0; // R 额外缩放因子（增强平滑，§8.2）
|
||||
|
||||
private:
|
||||
std::array<double, 8> x_{}; // 状态向量（§3.1）
|
||||
std::array<std::array<double, 8>, 8> P_{}; // 状态协方差 8×8（§3.1）
|
||||
bool initialized_ = false; // 是否已初始化（§3.1）
|
||||
|
||||
// 状态转移矩阵 F（常量，§4.1）：I4 + 右上 I4，即 F[i][i]=1、F[i][i+4]=1
|
||||
static constexpr std::array<std::array<double, 8>, 8> kF = {{
|
||||
{{1, 0, 0, 0, 1, 0, 0, 0}},
|
||||
{{0, 1, 0, 0, 0, 1, 0, 0}},
|
||||
{{0, 0, 1, 0, 0, 0, 1, 0}},
|
||||
{{0, 0, 0, 1, 0, 0, 0, 1}},
|
||||
{{0, 0, 0, 0, 1, 0, 0, 0}},
|
||||
{{0, 0, 0, 0, 0, 1, 0, 0}},
|
||||
{{0, 0, 0, 0, 0, 0, 1, 0}},
|
||||
{{0, 0, 0, 0, 0, 0, 0, 1}},
|
||||
}};
|
||||
|
||||
// 观测矩阵 H（常量，§4.1）：[I4 | 0]（4 行 × 8 列），
|
||||
// 直接观测位置与尺寸、不观测速度
|
||||
static constexpr std::array<std::array<double, 8>, 4> kH = {{
|
||||
{{1, 0, 0, 0, 0, 0, 0, 0}},
|
||||
{{0, 1, 0, 0, 0, 0, 0, 0}},
|
||||
{{0, 0, 1, 0, 0, 0, 0, 0}},
|
||||
{{0, 0, 0, 1, 0, 0, 0, 0}},
|
||||
}};
|
||||
};
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,123 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// target_tracker.h —— 多目标追踪过滤器（TargetTracker）对外接口
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §2 输入输出接口定义（Detection / TrackResult）
|
||||
// §3.3 TargetTracker（管理器）
|
||||
// §5 检测-轨迹关联（贪婪最邻近，§5.3）
|
||||
// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
|
||||
// §7 主流程（每帧 Update 六步）
|
||||
//
|
||||
// 线程模型（§9.5）：Update() 由单线程（推理主循环）调用；库内仅对
|
||||
// last_result 缓存加一把 std::mutex，LastResult() 返回受保护拷贝。
|
||||
// 本库零第三方依赖（不依赖 spdlog / Eigen / OpenCV）。
|
||||
// =============================================================================
|
||||
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "tracklet.h"
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
/// 单个检测框（§2.2 输入 detections 元素：[x1,y1,x2,y2,conf,cls_id]）
|
||||
struct Detection {
|
||||
double x1 = 0.0; // 边界框左上角 x（像素坐标）
|
||||
double y1 = 0.0; // 边界框左上角 y
|
||||
double x2 = 0.0; // 边界框右下角 x
|
||||
double y2 = 0.0; // 边界框右下角 y
|
||||
double conf = 0.0; // 检测置信度 ∈ [0,1]
|
||||
int cls_id = 0; // 类别编号
|
||||
};
|
||||
|
||||
/// 画面尺寸（height, width，§2.2）：仅在"无实测轨道、只有 Coast 轨道"
|
||||
/// 时用于选取离画面中心最近的轨道；缺省按 640×480 中心 (320,240)。
|
||||
struct FrameShape {
|
||||
int height = 0;
|
||||
int width = 0;
|
||||
};
|
||||
|
||||
/// 每帧追踪输出（§2.3）
|
||||
struct TrackResult {
|
||||
bool tracked = false; // 是否存在有效主目标；false 时调用方应刹车
|
||||
std::optional<int> primary_id; // 主目标轨道 ID；无目标时为空
|
||||
std::pair<double, double> center{0.0, 0.0}; // 平滑后的目标中心点（tracked=false 时无效）
|
||||
std::optional<BoxXYXY> box; // 平滑后的边界框
|
||||
double confidence = 0.0; // 当前置信度（Coast 期间继承最近实测值）
|
||||
bool is_predicted = false; // true = Coast（仅预测、无实测）
|
||||
int lost_frames = 0; // 主目标连续未命中帧数
|
||||
int n_active = 0; // 当前活跃轨道总数
|
||||
std::optional<BoxXYXY> raw; // 关联到的原始检测框；Coast 时为空（§2.3）
|
||||
};
|
||||
|
||||
/// 多目标追踪过滤器（§1.1 / §3.3）
|
||||
class TargetTracker {
|
||||
public:
|
||||
/// 追踪器配置参数（§8.1，默认值取自规格）
|
||||
struct Config {
|
||||
int max_lost_frames = 8; // 最大连续丢失帧数（严格大于才判死，§6.1）
|
||||
double max_association_dist = 200.0; // 中心距离软阈值 Dmax（像素，§5.1）
|
||||
int min_hits = 3; // 轨道"确认"所需累计匹配次数（§6.1）
|
||||
double dist_weight = 0.5; // 关联代价距离项权重（§5.1）
|
||||
double iou_weight = 0.5; // 关联代价 (1−IoU) 项权重（§5.1）
|
||||
};
|
||||
|
||||
/// 默认配置构造（等价于 Config{}，§8.1 默认值）
|
||||
TargetTracker();
|
||||
|
||||
/// 指定配置构造
|
||||
explicit TargetTracker(const Config& cfg);
|
||||
|
||||
/// 每帧调用一次（§7 六步主流程）：输入检测列表与可选画面尺寸，
|
||||
/// 输出主目标追踪结果并缓存（供 LastResult 跨线程读取）。
|
||||
TrackResult Update(const std::vector<Detection>& detections,
|
||||
std::optional<FrameShape> frame_shape = std::nullopt);
|
||||
|
||||
/// 上一帧结果快照（线程安全：返回受互斥锁保护的拷贝，§9.5）
|
||||
TrackResult LastResult() const;
|
||||
|
||||
/// 清空所有轨道、ID 计数器、锁定状态与缓存结果（§2.1）；next_id 归零（§6.2）
|
||||
void Reset();
|
||||
|
||||
/// 当前存活轨道数量（§2.1）
|
||||
int ActiveCount() const;
|
||||
|
||||
/// 当前所有轨道的只读快照（§2.1；值拷贝）
|
||||
std::unordered_map<int, Tracklet> ActiveTracks() const;
|
||||
|
||||
/// 配置只读访问
|
||||
const Config& GetConfig() const { return config_; }
|
||||
|
||||
private:
|
||||
/// 贪婪关联结果（§5.3）：matches 元素为 (track_id, 检测索引)
|
||||
struct AssociationResult {
|
||||
std::vector<std::pair<int, int>> matches;
|
||||
std::vector<int> unmatched_track_ids;
|
||||
std::vector<int> unmatched_det_indices;
|
||||
};
|
||||
|
||||
/// 贪婪最邻近关联（§5.3）：按轨道 confidence 降序逐一匹配，
|
||||
/// cost = w_dist·dist/max(Dmax,1e-3) + w_iou·(1−IoU)，仅 cost 严格小于
|
||||
/// best_cost（初始 1.0）才匹配，即 cost ≥ 1.0 判为不可匹配（§5.1）。
|
||||
AssociationResult AssociateDetections(const std::vector<Detection>& detections);
|
||||
|
||||
/// 主目标选择/锁定延续（§6.3 SELECT_PRIMARY）
|
||||
TrackResult SelectPrimary(std::optional<FrameShape> frame_shape);
|
||||
|
||||
Config config_; // 配置参数（§3.3）
|
||||
int next_id_ = 0; // 下一个可分配 ID，从 0 递增不复用（§6.2）
|
||||
std::unordered_map<int, Tracklet> tracks_; // 所有存活轨道（§3.3）
|
||||
long long frame_count_ = 0; // 已处理帧数（仅统计用，§3.3）
|
||||
std::optional<int> locked_primary_id_; // 当前锁定的主目标 ID（§3.3）
|
||||
TrackResult last_result_; // 上一帧结果缓存（§3.3）
|
||||
mutable std::mutex last_result_mutex_; // 仅保护 last_result_（§9.5）
|
||||
};
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,93 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// tracklet.h —— 轨道（Tracklet）与基础几何工具
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §3.2 Tracklet（轨道）字段定义与派生状态
|
||||
// §5.2 IoU 计算
|
||||
// §7 主流程伪代码末尾 TRACKLET_PREDICT / TRACKLET_UPDATE
|
||||
//
|
||||
// 几何量说明：BoxCXCYWH（卡尔曼观测空间 [cx,cy,w,h]）定义于
|
||||
// kalman_box_filter.h，本文件通过 include 复用并同时提供 BoxXYXY
|
||||
// （像素坐标 [x1,y1,x2,y2]）与两种表示的互转函数。
|
||||
// 本库只依赖 C++ 标准库，无任何第三方依赖。
|
||||
// =============================================================================
|
||||
|
||||
#include <optional>
|
||||
#include <utility>
|
||||
|
||||
#include "kalman_box_filter.h"
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
// ---- 基础几何量（§9.1 模块/类划分草案） ----
|
||||
|
||||
/// 轴对齐边界框（像素坐标，左上/右下角表示）
|
||||
struct BoxXYXY {
|
||||
double x1 = 0.0;
|
||||
double y1 = 0.0;
|
||||
double x2 = 0.0;
|
||||
double y2 = 0.0;
|
||||
};
|
||||
|
||||
/// [x1,y1,x2,y2] → [cx,cy,w,h]（§7 TRACKLET_UPDATE 中的转换）
|
||||
BoxCXCYWH ToCenterSize(const BoxXYXY& box);
|
||||
|
||||
/// [cx,cy,w,h] → [x1,y1,x2,y2]（§7 TRACKLET_PREDICT 中的 center_size 逆变换）
|
||||
BoxXYXY ToXYXY(const BoxCXCYWH& box);
|
||||
|
||||
/// 标准轴对齐 IoU（§5.2）：
|
||||
/// 任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。
|
||||
double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b);
|
||||
|
||||
// =============================================================================
|
||||
// Tracklet —— 单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）
|
||||
// =============================================================================
|
||||
struct Tracklet {
|
||||
/// 构造即创建轨道（§7 步骤 5 / §6.1 新建阶段）：
|
||||
/// 用首次观测初始化滤波器，age=1、hits=1、lost_count=0，
|
||||
/// last_center / last_box 立即由观测重建（保证创建当帧即可参与输出）。
|
||||
Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
|
||||
int max_lost_frames);
|
||||
|
||||
int track_id = 0; // 全局唯一轨道 ID（§3.2）
|
||||
KalmanBoxFilter kf; // 本轨道的滤波器（§3.2）
|
||||
int lost_count = 0; // 当前连续未匹配帧数（§3.2）
|
||||
int max_lost = 0; // 最大允许连续丢失帧数（构造时注入，§3.2）
|
||||
double confidence = 0.0; // 最近一次匹配的置信度（§3.2）
|
||||
int cls_id = 0; // 最近一次匹配的类别 ID（§3.2）
|
||||
int age = 1; // 轨道已存在的总帧数（创建时为 1，§3.2）
|
||||
int hits = 1; // 累计成功匹配次数（创建时为 1，§3.2）
|
||||
std::pair<double, double> last_center{0.0, 0.0}; // 最近一次中心点（实测或预测）
|
||||
std::optional<BoxXYXY> last_box; // 最近一次边界框（实测或预测，§3.2）
|
||||
std::optional<BoxXYXY> last_raw_box; // 最近一次实测关联到的原始检测框；
|
||||
// Coast 期间保留不更新，仅内部参考；
|
||||
// 对外输出语义按 §2.3：Coast 时 raw 置空
|
||||
|
||||
// ---- 派生状态（§3.2 只读属性） ----
|
||||
|
||||
/// is_dead := lost_count > max_lost（严格大于，§6.1 注记 2）
|
||||
bool IsDead() const { return lost_count > max_lost; }
|
||||
|
||||
/// is_coasting := lost_count > 0（处于纯预测状态，§3.2）
|
||||
bool IsCoasting() const { return lost_count > 0; }
|
||||
|
||||
// ---- 生命周期方法（§7 末尾伪代码） ----
|
||||
|
||||
/// TRACKLET_PREDICT：卡尔曼预测推进一帧。
|
||||
/// 预测成功时更新 last_center / last_box 且 age+1。
|
||||
void Predict();
|
||||
|
||||
/// TRACKLET_UPDATE：框 [x1,y1,x2,y2] → [cx,cy,w,h] → kf.update；
|
||||
/// lost_count 清零、hits+1、age+1，更新
|
||||
/// last_center / last_box / last_raw_box / confidence / cls_id。
|
||||
void Update(const BoxXYXY& box, double conf, int cls);
|
||||
|
||||
/// 未匹配计丢失：lost_count+1、age+1（§7 步骤 4）。
|
||||
void MarkMiss();
|
||||
};
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
Reference in New Issue
Block a user