first commit
This commit is contained in:
@@ -0,0 +1,235 @@
|
||||
// =============================================================================
|
||||
// kalman_box_filter.cpp —— 单目标 8 维恒定速度卡尔曼滤波器实现
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md §4（算法细节）、§8.2（内部常量）、
|
||||
// §9.3（数值计算：定长数组手写矩阵乘法，4×4 高斯-约当求逆 + 1e-9 正则化）。
|
||||
// 关键公式逐行注释并标注对照文档小节号。
|
||||
// =============================================================================
|
||||
|
||||
#include "target_tracker/kalman_box_filter.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <utility>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
namespace {
|
||||
|
||||
// ---- 定长矩阵基础运算（§9.3：无需引入 Eigen/OpenCV） ----
|
||||
|
||||
/// 8×8 矩阵乘法：C = A·B
|
||||
std::array<std::array<double, 8>, 8> MatMul8(const std::array<std::array<double, 8>, 8>& a,
|
||||
const std::array<std::array<double, 8>, 8>& b) {
|
||||
std::array<std::array<double, 8>, 8> c{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int k = 0; k < 8; ++k) {
|
||||
if (a[i][k] == 0.0) continue; // 稀疏常量矩阵加速，结果不变
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
c[i][j] += a[i][k] * b[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
/// 8×8 矩阵转置：R = Aᵀ
|
||||
std::array<std::array<double, 8>, 8> MatTranspose8(
|
||||
const std::array<std::array<double, 8>, 8>& a) {
|
||||
std::array<std::array<double, 8>, 8> r{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
r[i][j] = a[j][i];
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
/// 4×4 矩阵求逆：高斯-约当消元 + 部分主元（§9.3）。
|
||||
/// 输入矩阵先对角加 1e-9 正则化防奇异（§9.3 建议）。
|
||||
/// 返回的 bool 表示是否求逆成功（主元退化时返回 false）。
|
||||
bool Invert4x4(const std::array<std::array<double, 4>, 4>& m,
|
||||
std::array<std::array<double, 4>, 4>& out) {
|
||||
constexpr double kRegularization = 1e-9; // 正则化极小量（§9.3）
|
||||
|
||||
// 增广矩阵 [M + εI | I]，消元后左半变为单位阵、右半即为逆矩阵
|
||||
std::array<std::array<double, 8>, 4> aug{};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
aug[i][j] = m[i][j] + (i == j ? kRegularization : 0.0);
|
||||
aug[i][j + 4] = (i == j) ? 1.0 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
for (int col = 0; col < 4; ++col) {
|
||||
// 部分主元：选取当前列绝对值最大行交换，提高数值稳定性
|
||||
int pivot = col;
|
||||
for (int r = col + 1; r < 4; ++r) {
|
||||
if (std::abs(aug[r][col]) > std::abs(aug[pivot][col])) {
|
||||
pivot = r;
|
||||
}
|
||||
}
|
||||
if (std::abs(aug[pivot][col]) < 1e-12) {
|
||||
return false; // 主元退化（正则化后理论上不可达）
|
||||
}
|
||||
if (pivot != col) {
|
||||
std::swap(aug[pivot], aug[col]);
|
||||
}
|
||||
// 主元行归一化
|
||||
const double d = aug[col][col];
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
aug[col][j] /= d;
|
||||
}
|
||||
// 消去其余行（高斯-约当：一次消成对角单位阵）
|
||||
for (int r = 0; r < 4; ++r) {
|
||||
if (r == col) continue;
|
||||
const double f = aug[r][col];
|
||||
if (f == 0.0) continue;
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
aug[r][j] -= f * aug[col][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
out[i][j] = aug[i][j + 4];
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void KalmanBoxFilter::Initiate(const BoxCXCYWH& z) {
|
||||
// §4.3 初始化：状态 = [cx, cy, w, h, 0, 0, 0, 0]（速度置零）
|
||||
x_ = {{z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0}};
|
||||
|
||||
// §4.3 协方差：P = diag(2,2,2,2,100,100,100,100)
|
||||
// （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计）
|
||||
for (auto& row : P_) row.fill(0.0);
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
P_[i][i] = kIniPos; // INI_POS = 2.0（§4.2）
|
||||
P_[i + 4][i + 4] = kIniVel; // INI_VEL = 100.0（§4.2）
|
||||
}
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
std::optional<BoxCXCYWH> KalmanBoxFilter::Predict() {
|
||||
// §4.4：未初始化返回空
|
||||
if (!initialized_) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
// §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放
|
||||
// σ_pos = [w/50, h/50, w/50, h/50]
|
||||
// σ_vel = [w/200, h/200, w/200, h/200]
|
||||
// （Q 直接使用状态值，不做 abs，与规格 §4.2 注记保持一致）
|
||||
const double w = x_[2];
|
||||
const double h = x_[3];
|
||||
const double std_pos[4] = {w * kStdPos, h * kStdPos, w * kStdPos, h * kStdPos};
|
||||
const double std_vel[4] = {w * kStdVel, h * kStdVel, w * kStdVel, h * kStdVel};
|
||||
std::array<std::array<double, 8>, 8> q{};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
q[i][i] = std_pos[i] * std_pos[i]; // 前 4 对角元来自 σ_pos（各分量先平方）
|
||||
q[i + 4][i + 4] = std_vel[i] * std_vel[i]; // 后 4 对角元来自 σ_vel
|
||||
}
|
||||
|
||||
// §4.4 状态转移：x ← F·x（恒定速度模型：位置/尺寸各自加上对应速度）
|
||||
std::array<double, 8> x_new{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
double sum = 0.0;
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
sum += kF[i][j] * x_[j];
|
||||
}
|
||||
x_new[i] = sum;
|
||||
}
|
||||
x_ = x_new;
|
||||
|
||||
// §4.4 协方差传播：P ← F·P·Fᵀ + Q
|
||||
P_ = MatMul8(MatMul8(kF, P_), MatTranspose8(kF));
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
P_[i][j] += q[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// §4.4 返回预测的位置/尺寸（状态向量前 4 维的副本）
|
||||
return BoxCXCYWH{x_[0], x_[1], x_[2], x_[3]};
|
||||
}
|
||||
|
||||
void KalmanBoxFilter::Update(const BoxCXCYWH& z) {
|
||||
// §4.5：未初始化直接返回
|
||||
if (!initialized_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// §4.2 动态观测噪声：R = diag(2(w/50)², 2(h/50)², 2(w/50)², 2(h/50)²)
|
||||
// （R 使用 abs(w)/abs(h) 防御负尺寸，并额外乘 2 增强平滑；
|
||||
// 该差异与规格 §4.2 注记保持一致）
|
||||
const double w = std::abs(x_[2]);
|
||||
const double h = std::abs(x_[3]);
|
||||
std::array<std::array<double, 4>, 4> r{};
|
||||
const double rw = kRScale * (w * kStdPos) * (w * kStdPos);
|
||||
const double rh = kRScale * (h * kStdPos) * (h * kStdPos);
|
||||
r[0][0] = rw;
|
||||
r[1][1] = rh;
|
||||
r[2][2] = rw;
|
||||
r[3][3] = rh;
|
||||
|
||||
// §4.5 新息协方差：S = H·P·Hᵀ + R（4×4）
|
||||
// H = [I4 | 0]，故 H·P·Hᵀ 即 P 的左上 4×4 子块
|
||||
std::array<std::array<double, 4>, 4> s{};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
s[i][j] = P_[i][j] + r[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// §4.5 / §9.3：S⁻¹（4×4 高斯-约当求逆 + 对角加 1e-9 正则化防奇异）
|
||||
std::array<std::array<double, 4>, 4> s_inv{};
|
||||
if (!Invert4x4(s, s_inv)) {
|
||||
return; // 正则化后理论不可达；防御性跳过本次更新
|
||||
}
|
||||
|
||||
// §4.5 卡尔曼增益：K = P·Hᵀ·S⁻¹（8×4）
|
||||
// H = [I4 | 0]，故 P·Hᵀ 即 P 的前 4 列（8×4）
|
||||
std::array<std::array<double, 4>, 8> k{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
double sum = 0.0;
|
||||
for (int l = 0; l < 4; ++l) {
|
||||
sum += P_[i][l] * s_inv[l][j];
|
||||
}
|
||||
k[i][j] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
// §4.5 残差（innovation）：y = z − H·x（H·x 即状态前 4 维）
|
||||
const double z_vec[4] = {z.cx, z.cy, z.w, z.h};
|
||||
double y[4];
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
y[i] = z_vec[i] - x_[i];
|
||||
}
|
||||
|
||||
// §4.5 状态更新：x ← x + K·y
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
double sum = 0.0;
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
sum += k[i][j] * y[j];
|
||||
}
|
||||
x_[i] += sum;
|
||||
}
|
||||
|
||||
// §4.5 协方差更新：P ← (I8 − K·H)·P
|
||||
// K·H（8×8）= K 的前 4 列铺到前 4 列上（H 选通前 4 维）
|
||||
std::array<std::array<double, 8>, 8> ikh{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
ikh[i][j] = (i == j ? 1.0 : 0.0) - (j < 4 ? k[i][j] : 0.0);
|
||||
}
|
||||
}
|
||||
P_ = MatMul8(ikh, P_);
|
||||
}
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,312 @@
|
||||
// =============================================================================
|
||||
// target_tracker.cpp —— 多目标追踪过滤器（TargetTracker）实现
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §5 检测-轨迹关联（贪婪最邻近，§5.3）
|
||||
// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
|
||||
// §7 主流程六步（UPDATE 伪代码）
|
||||
// §9.5 线程模型（单线程写 Update，仅 last_result 缓存加锁）
|
||||
// =============================================================================
|
||||
|
||||
#include "target_tracker/target_tracker.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
TargetTracker::TargetTracker() : TargetTracker(Config{}) {}
|
||||
|
||||
TargetTracker::TargetTracker(const Config& cfg) : config_(cfg) {}
|
||||
|
||||
TrackResult TargetTracker::Update(const std::vector<Detection>& detections,
|
||||
std::optional<FrameShape> frame_shape) {
|
||||
// §7：frame_count += 1（仅统计用）
|
||||
++frame_count_;
|
||||
|
||||
// ---- 入口防御：过滤坐标或置信度含 NaN/Inf 的检测 ----
|
||||
// 防御性增强：上游检测器理论上已过滤，但库自身必须免疫——
|
||||
// 非有限值一旦进入卡尔曼状态将永久污染轨道并传导到主目标输出
|
||||
std::vector<Detection> valid_detections;
|
||||
valid_detections.reserve(detections.size());
|
||||
for (const Detection& det : detections) {
|
||||
if (std::isfinite(det.x1) && std::isfinite(det.y1) &&
|
||||
std::isfinite(det.x2) && std::isfinite(det.y2) &&
|
||||
std::isfinite(det.conf)) {
|
||||
valid_detections.push_back(det);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 步骤 1：所有存活轨道先行预测（推进一帧）----
|
||||
// （§7 步骤 1：t.predict() 更新 last_center / last_box、age += 1）
|
||||
for (auto& kv : tracks_) {
|
||||
kv.second.Predict();
|
||||
}
|
||||
|
||||
// ---- 步骤 2：构造检测中心表并贪婪关联（§7 步骤 2 / §5.3） ----
|
||||
AssociationResult assoc;
|
||||
if (!valid_detections.empty()) {
|
||||
assoc = AssociateDetections(valid_detections);
|
||||
} else {
|
||||
// 无检测：全部轨道视为未匹配，全部进入 Coast 计数（§7 步骤 2 否则分支）
|
||||
for (const auto& kv : tracks_) {
|
||||
assoc.unmatched_track_ids.push_back(kv.first);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 步骤 3：用匹配到的检测更新轨道（§7 步骤 3） ----
|
||||
for (const auto& match : assoc.matches) {
|
||||
const int track_id = match.first;
|
||||
const Detection& det = valid_detections[static_cast<size_t>(match.second)];
|
||||
// §7 步骤 3：由检测重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
|
||||
// （等价于检测框本身：cx=(x1+x2)/2、w=x2−x1 的逆运算，浮点上精确还原）
|
||||
const BoxXYXY raw_box{det.x1, det.y1, det.x2, det.y2};
|
||||
tracks_.at(track_id).Update(raw_box, det.conf, det.cls_id);
|
||||
}
|
||||
|
||||
// ---- 步骤 4：未匹配轨道计丢失，死亡者回收（§7 步骤 4） ----
|
||||
for (const int track_id : assoc.unmatched_track_ids) {
|
||||
auto it = tracks_.find(track_id);
|
||||
if (it == tracks_.end()) {
|
||||
continue;
|
||||
}
|
||||
it->second.MarkMiss(); // lost_count+1、age+1
|
||||
if (it->second.IsDead()) { // lost_count > max_lost（严格大于，§6.1）
|
||||
// 锁定的唯一解除途径之一：锁定轨道死亡被回收时清锁（§6.3）
|
||||
if (locked_primary_id_.has_value() && *locked_primary_id_ == track_id) {
|
||||
locked_primary_id_.reset();
|
||||
}
|
||||
tracks_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 步骤 5：未匹配检测 → 创建新轨道（§7 步骤 5 / §6.2） ----
|
||||
for (const int det_idx : assoc.unmatched_det_indices) {
|
||||
const Detection& det = valid_detections[static_cast<size_t>(det_idx)];
|
||||
// §7 步骤 5：以 [cx,cy,w,h] 创建；构造函数内重建 last_box，
|
||||
// 保证创建当帧即可参与输出；next_id 从 0 递增永不复用（§6.2）
|
||||
tracks_.emplace(next_id_,
|
||||
Tracklet(next_id_, ToCenterSize({det.x1, det.y1, det.x2, det.y2}),
|
||||
det.conf, det.cls_id, config_.max_lost_frames));
|
||||
++next_id_;
|
||||
}
|
||||
|
||||
// ---- 步骤 6：选取/延续主目标，组装输出并缓存（§7 步骤 6 / §6.3） ----
|
||||
TrackResult result = SelectPrimary(frame_shape);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(last_result_mutex_);
|
||||
last_result_ = result; // 缓存供跨线程读取（§9.5）
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
TrackResult TargetTracker::LastResult() const {
|
||||
// §9.5：LastResult() 返回受互斥锁保护的拷贝
|
||||
std::lock_guard<std::mutex> lock(last_result_mutex_);
|
||||
return last_result_;
|
||||
}
|
||||
|
||||
void TargetTracker::Reset() {
|
||||
// §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果
|
||||
tracks_.clear();
|
||||
next_id_ = 0; // §6.2：reset() 时计数器归零
|
||||
frame_count_ = 0;
|
||||
locked_primary_id_.reset(); // 锁定的唯一解除途径之二（§6.3）
|
||||
std::lock_guard<std::mutex> lock(last_result_mutex_);
|
||||
last_result_ = TrackResult{};
|
||||
}
|
||||
|
||||
int TargetTracker::ActiveCount() const {
|
||||
return static_cast<int>(tracks_.size());
|
||||
}
|
||||
|
||||
std::unordered_map<int, Tracklet> TargetTracker::ActiveTracks() const {
|
||||
return tracks_; // 值拷贝只读快照（§2.1）
|
||||
}
|
||||
|
||||
TargetTracker::AssociationResult TargetTracker::AssociateDetections(
|
||||
const std::vector<Detection>& detections) {
|
||||
AssociationResult result;
|
||||
|
||||
// §5.3：按轨道 confidence 降序排序（复制 ID 到 vector 后 sort，
|
||||
// 不直接排哈希表，§9.2）
|
||||
std::vector<int> sorted_ids;
|
||||
sorted_ids.reserve(tracks_.size());
|
||||
for (const auto& kv : tracks_) {
|
||||
sorted_ids.push_back(kv.first);
|
||||
}
|
||||
std::sort(sorted_ids.begin(), sorted_ids.end(), [this](int a, int b) {
|
||||
const double raw_a = tracks_.at(a).confidence;
|
||||
const double raw_b = tracks_.at(b).confidence;
|
||||
// NaN 安全：非有限置信度按 −∞ 处理（排最后），避免 NaN 参与比较
|
||||
// 破坏 std::sort 要求的严格弱序（未定义行为）；映射后为全序，
|
||||
// 等价传递性恒成立
|
||||
const double ca = std::isfinite(raw_a)
|
||||
? raw_a
|
||||
: -std::numeric_limits<double>::infinity();
|
||||
const double cb = std::isfinite(raw_b)
|
||||
? raw_b
|
||||
: -std::numeric_limits<double>::infinity();
|
||||
if (ca != cb) return ca > cb; // 置信度从高到低
|
||||
return a < b; // 平局按 ID 升序作次级键，保证确定性
|
||||
});
|
||||
|
||||
std::vector<bool> det_assigned(detections.size(), false);
|
||||
|
||||
for (const int track_id : sorted_ids) {
|
||||
const Tracklet& track = tracks_.at(track_id);
|
||||
// §5.3：每条轨道最优代价初始化为 1.0（仅 cost 严格小于 1.0 才记录，
|
||||
// 即 cost ≥ 1.0 判为不可匹配，§5.1 匹配有效性判据）
|
||||
double best_cost = 1.0;
|
||||
int best_det = -1;
|
||||
for (size_t d = 0; d < detections.size(); ++d) {
|
||||
if (det_assigned[d]) continue; // 跳过已分配检测
|
||||
const Detection& det = detections[d];
|
||||
// §5.1：中心点欧氏距离（像素）
|
||||
const double det_cx = (det.x1 + det.x2) / 2.0;
|
||||
const double det_cy = (det.y1 + det.y2) / 2.0;
|
||||
const double dist =
|
||||
std::hypot(track.last_center.first - det_cx,
|
||||
track.last_center.second - det_cy);
|
||||
// §5.1：归一化距离，取 max(Dmax, 1e-3) 防除零
|
||||
const double norm_dist =
|
||||
dist / std::max(config_.max_association_dist, 1e-3);
|
||||
// §5.2：预测框与检测框的 IoU
|
||||
const double iou = track.last_box.has_value()
|
||||
? ComputeIoU(*track.last_box,
|
||||
{det.x1, det.y1, det.x2, det.y2})
|
||||
: 0.0;
|
||||
// §5.1：cost = w_dist·norm_dist + w_iou·(1−IoU)
|
||||
const double cost = config_.dist_weight * norm_dist +
|
||||
config_.iou_weight * (1.0 - iou);
|
||||
if (cost < best_cost) { // 严格小于才更新（§5.1 / §5.3）
|
||||
best_cost = cost;
|
||||
best_det = static_cast<int>(d);
|
||||
}
|
||||
}
|
||||
if (best_det >= 0) {
|
||||
result.matches.emplace_back(track_id, best_det);
|
||||
det_assigned[static_cast<size_t>(best_det)] = true;
|
||||
} else {
|
||||
result.unmatched_track_ids.push_back(track_id);
|
||||
}
|
||||
}
|
||||
|
||||
// §5.3：未分配检测收集
|
||||
for (size_t d = 0; d < detections.size(); ++d) {
|
||||
if (!det_assigned[d]) {
|
||||
result.unmatched_det_indices.push_back(static_cast<int>(d));
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
TrackResult TargetTracker::SelectPrimary(std::optional<FrameShape> frame_shape) {
|
||||
TrackResult result;
|
||||
result.n_active = static_cast<int>(tracks_.size());
|
||||
|
||||
// §6.3：轨道表为空 → 清除锁定，返回 tracked=false
|
||||
if (tracks_.empty()) {
|
||||
locked_primary_id_.reset();
|
||||
return result;
|
||||
}
|
||||
|
||||
// §6.3 ① 锁定延续：锁定轨道仍存活则直接输出（Coast 中照样输出，
|
||||
// is_predicted=true；不被其他更高置信度目标抢占）
|
||||
Tracklet* chosen = nullptr;
|
||||
bool is_predicted = false;
|
||||
if (locked_primary_id_.has_value()) {
|
||||
auto it = tracks_.find(*locked_primary_id_);
|
||||
if (it != tracks_.end()) {
|
||||
chosen = &it->second;
|
||||
is_predicted = it->second.IsCoasting();
|
||||
}
|
||||
}
|
||||
|
||||
// §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
|
||||
if (chosen == nullptr) {
|
||||
// measured ← 所有 lost_count == 0 的轨道；coasting ← 其余存活轨道
|
||||
std::vector<Tracklet*> measured;
|
||||
std::vector<Tracklet*> coasting;
|
||||
for (auto& kv : tracks_) {
|
||||
if (kv.second.IsCoasting()) {
|
||||
coasting.push_back(&kv.second);
|
||||
} else {
|
||||
measured.push_back(&kv.second);
|
||||
}
|
||||
}
|
||||
|
||||
if (!measured.empty()) {
|
||||
// confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
|
||||
// （min_hits 只影响候选池优先级，不阻止输出，§6.1 注记 1）
|
||||
std::vector<Tracklet*> pool;
|
||||
for (Tracklet* t : measured) {
|
||||
if (t->hits >= config_.min_hits) {
|
||||
pool.push_back(t);
|
||||
}
|
||||
}
|
||||
if (pool.empty()) {
|
||||
pool = measured;
|
||||
}
|
||||
// chosen ← pool 中 confidence 最大者；平局取 unordered_map 遍历序
|
||||
// 中首个最大者，不做确定性承诺（规格 §6.3 未规定平局规则）
|
||||
chosen = pool.front();
|
||||
for (Tracklet* t : pool) {
|
||||
if (t->confidence > chosen->confidence) {
|
||||
chosen = t;
|
||||
}
|
||||
}
|
||||
is_predicted = false;
|
||||
} else if (!coasting.empty()) {
|
||||
// 无实测轨道：选 coasting 中离画面中心最近者（欧氏距离平方排序）
|
||||
// §2.2：frame_shape 缺省按 640×480 画面中心 (320, 240)
|
||||
const double center_x =
|
||||
(frame_shape.has_value() && frame_shape->width > 0)
|
||||
? frame_shape->width / 2.0
|
||||
: 320.0;
|
||||
const double center_y =
|
||||
(frame_shape.has_value() && frame_shape->height > 0)
|
||||
? frame_shape->height / 2.0
|
||||
: 240.0;
|
||||
double best_dist_sq = std::numeric_limits<double>::infinity();
|
||||
for (Tracklet* t : coasting) {
|
||||
const double dx = t->last_center.first - center_x;
|
||||
const double dy = t->last_center.second - center_y;
|
||||
const double dist_sq = dx * dx + dy * dy;
|
||||
if (dist_sq < best_dist_sq) {
|
||||
best_dist_sq = dist_sq;
|
||||
chosen = t;
|
||||
}
|
||||
}
|
||||
if (chosen == nullptr) {
|
||||
// NaN 残余的防御兜底：若所有 coasting 轨道中心均为 NaN，
|
||||
// dist_sq 恒为 NaN 无人当选，取首个避免空指针解引用；
|
||||
// 正常输入已在 Update 入口经有限性过滤，本分支理论上不可达
|
||||
chosen = coasting.front();
|
||||
}
|
||||
is_predicted = true; // Coast 兜底：仅预测、无实测
|
||||
} else {
|
||||
// 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
|
||||
locked_primary_id_.reset();
|
||||
return result;
|
||||
}
|
||||
locked_primary_id_ = chosen->track_id; // 选举后锁定（§6.3）
|
||||
}
|
||||
|
||||
// ---- 组装输出（§2.3 九字段） ----
|
||||
result.tracked = true;
|
||||
result.primary_id = chosen->track_id;
|
||||
result.center = chosen->last_center;
|
||||
result.box = chosen->last_box;
|
||||
result.confidence = chosen->confidence; // Coast 期间继承最近实测置信度
|
||||
result.is_predicted = is_predicted;
|
||||
result.lost_frames = chosen->lost_count;
|
||||
// §2.3：raw = 关联到的原始检测框；Coast 时为空（锁定延续分支同样遵守）
|
||||
result.raw = is_predicted ? std::nullopt : chosen->last_raw_box;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,101 @@
|
||||
// =============================================================================
|
||||
// tracklet.cpp —— 轨道（Tracklet）与基础几何工具实现
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §5.2 IoU 计算
|
||||
// §7 TRACKLET_PREDICT / TRACKLET_UPDATE 伪代码
|
||||
// §3.2 Tracklet 字段语义
|
||||
// =============================================================================
|
||||
|
||||
#include "target_tracker/tracklet.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
BoxCXCYWH ToCenterSize(const BoxXYXY& box) {
|
||||
// §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
|
||||
return BoxCXCYWH{(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
|
||||
box.x2 - box.x1, box.y2 - box.y1};
|
||||
}
|
||||
|
||||
BoxXYXY ToXYXY(const BoxCXCYWH& box) {
|
||||
// §7 TRACKLET_PREDICT：center_size → [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
|
||||
return BoxXYXY{box.cx - box.w / 2.0, box.cy - box.h / 2.0,
|
||||
box.cx + box.w / 2.0, box.cy + box.h / 2.0};
|
||||
}
|
||||
|
||||
double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b) {
|
||||
// §5.2：任一框为空（宽或高 ≤ 0）返回 0
|
||||
const double area_a = (a.x2 - a.x1) * (a.y2 - a.y1);
|
||||
const double area_b = (b.x2 - b.x1) * (b.y2 - b.y1);
|
||||
if (area_a <= 0.0 || area_b <= 0.0) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// 交集宽高与 0 取最大值（§5.2）
|
||||
const double inter_w = std::min(a.x2, b.x2) - std::max(a.x1, b.x1);
|
||||
const double inter_h = std::min(a.y2, b.y2) - std::max(a.y1, b.y1);
|
||||
if (inter_w <= 0.0 || inter_h <= 0.0) {
|
||||
return 0.0; // 交集面积 ≤ 0 返回 0（§5.2）
|
||||
}
|
||||
|
||||
const double inter_area = inter_w * inter_h;
|
||||
const double union_area = area_a + area_b - inter_area;
|
||||
if (union_area <= 0.0) {
|
||||
return 0.0; // 并集 ≤ 0 返回 0（§5.2）
|
||||
}
|
||||
// IoU = area(A∩B) / (area(A)+area(B)−area(A∩B))（§5.2）
|
||||
return inter_area / union_area;
|
||||
}
|
||||
|
||||
Tracklet::Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
|
||||
int max_lost_frames)
|
||||
: track_id(id),
|
||||
max_lost(max_lost_frames),
|
||||
confidence(conf),
|
||||
cls_id(cls) {
|
||||
// §4.3：用首次观测初始化滤波器（速度置零、P 取默认对角）
|
||||
kf.Initiate(measurement);
|
||||
// §7 步骤 5：创建当帧即赋 last_center / last_box（保证当帧可参与输出）
|
||||
last_center = {measurement.cx, measurement.cy};
|
||||
last_box = ToXYXY(measurement);
|
||||
// 创建即源于实测检测：同步记录原始检测框（Coast 期间保留，
|
||||
// 对外输出按 §2.3 在 Coast 时置空）
|
||||
last_raw_box = last_box;
|
||||
// age / hits / lost_count 由类内默认值保证（1 / 1 / 0，§3.2）
|
||||
}
|
||||
|
||||
void Tracklet::Predict() {
|
||||
// §7 TRACKLET_PREDICT：卡尔曼预测推进一帧
|
||||
const auto pred = kf.Predict();
|
||||
if (pred.has_value()) {
|
||||
// 预测成功：更新 last_center / last_box，age += 1
|
||||
last_center = {pred->cx, pred->cy};
|
||||
last_box = ToXYXY(*pred);
|
||||
++age;
|
||||
}
|
||||
}
|
||||
|
||||
void Tracklet::Update(const BoxXYXY& box, double conf, int cls) {
|
||||
// §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
|
||||
kf.Update(ToCenterSize(box));
|
||||
last_center = {(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0};
|
||||
last_box = box;
|
||||
last_raw_box = box; // 实测关联到的原始检测框（Coast 期间保留不更新）
|
||||
confidence = conf;
|
||||
cls_id = cls;
|
||||
lost_count = 0; // 匹配成功：连续丢失计数清零
|
||||
++hits; // 累计匹配次数 +1
|
||||
++age; // 帧龄 +1
|
||||
}
|
||||
|
||||
void Tracklet::MarkMiss() {
|
||||
// §7 步骤 4：未匹配计丢失 lost_count+1、age+1
|
||||
++lost_count;
|
||||
++age;
|
||||
}
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
Reference in New Issue
Block a user