songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
songhaoyu
2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
reference-c++/target_tracker/CMakeLists.txt
+85
View File
@@ -0,0 +1,85 @@
cmake_minimum_required(VERSION 3.16)
# =============================================================================
# target_tracker —— 视觉追踪库（独立动态库工程）
#
# 轻量级多目标追踪过滤器：恒定速度卡尔曼滤波 + 贪婪关联 + 主目标锁定。
# 规格见 docs/视觉追踪技术路线.md；实现文档见 target_tracker.md。
# 零第三方依赖（不引入 spdlog / Eigen / OpenCV），仅链接 C++ 标准库。
# =============================================================================
project(target_tracker VERSION 1.0.0 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# ---- 动态库目标 ----
add_library(target_tracker SHARED
src/kalman_box_filter.cpp
src/tracklet.cpp
src/target_tracker.cpp)
target_include_directories(target_tracker PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
target_compile_features(target_tracker PUBLIC cxx_std_17)
# 版本与符号：VERSION 1.0.0 / SOVERSION 1；默认符号可见性（不加
# visibility=hidden 与导出宏），全部公共类直接对外导出。
set_target_properties(target_tracker PROPERTIES
VERSION 1.0.0
SOVERSION 1)
# 编译告警：与主工程一致（非 MSVC：-Wall -Wextra -Wpedantic）
if(MSVC)
target_compile_options(target_tracker PRIVATE /W4 /permissive-)
else()
target_compile_options(target_tracker PRIVATE -Wall -Wextra -Wpedantic)
endif()
# ---- 单元测试（GTest；缺失时仅提示不失败，与主工程策略一致） ----
include(CTest)
if(BUILD_TESTING)
find_package(GTest QUIET)
if(GTest_FOUND)
include(GoogleTest)
# 测试可执行文件直接链接共享库 target_tracker；
# BUILD_RPATH 指向库输出目录，保证 ctest 无需 LD_LIBRARY_PATH 即可运行
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};$ORIGIN")
# 卡尔曼滤波器测试：初始化状态、静止收敛、匀速速度收敛与预测超前、未初始化行为
add_executable(kalman_box_filter_test tests/kalman_box_filter_test.cpp)
target_link_libraries(kalman_box_filter_test PRIVATE
target_tracker GTest::gtest_main)
if(MSVC)
target_compile_options(kalman_box_filter_test PRIVATE /W4 /permissive-)
else()
target_compile_options(kalman_box_filter_test PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(kalman_box_filter_test)
# 轨道与几何工具测试：IoU 边界、IsDead/IsCoasting、Predict/Update/MarkMiss 计数
add_executable(tracklet_test tests/tracklet_test.cpp)
target_link_libraries(tracklet_test PRIVATE
target_tracker GTest::gtest_main)
if(MSVC)
target_compile_options(tracklet_test PRIVATE /W4 /permissive-)
else()
target_compile_options(tracklet_test PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(tracklet_test)
# 追踪器测试：逐条覆盖文档 §9.4 六条行为一致性验收用例
add_executable(target_tracker_test tests/target_tracker_test.cpp)
target_link_libraries(target_tracker_test PRIVATE
target_tracker GTest::gtest_main)
if(MSVC)
target_compile_options(target_tracker_test PRIVATE /W4 /permissive-)
else()
target_compile_options(target_tracker_test PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(target_tracker_test)
else()
message(STATUS "GoogleTest not found; target_tracker tests will not be built")
endif()
endif()
reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h
+87
View File
@@ -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
reference-c++/target_tracker/include/target_tracker/target_tracker.h
+123
View File
@@ -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
reference-c++/target_tracker/include/target_tracker/tracklet.h
+93
View File
@@ -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-c++/target_tracker/src/kalman_box_filter.cpp
+235
View File
@@ -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
reference-c++/target_tracker/src/target_tracker.cpp
+312
View File
@@ -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
reference-c++/target_tracker/src/tracklet.cpp
+101
View File
@@ -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-c++/target_tracker/target_tracker.md
+217
View File
@@ -0,0 +1,217 @@
# target_tracker 视觉追踪库实现文档
> 独立动态库（`libtarget_tracker.so`），实现轻量级多目标追踪过滤器：
> 恒定速度卡尔曼滤波平滑 + Coast 抗短暂丢失 + ID 稳定与主目标锁定。
> 算法行为的唯一权威规格为 `docs/视觉追踪技术路线.md`（下称"技术路线"），
> 本文档不重复抄录算法全文，仅描述实现结构与对照关系。
---
## 一、功能职责与边界
**做什么**：
- 每帧接收 YOLO 检测框列表，输出**一个主目标**的平滑中心点/边界框/状态标志
（`TrackResult`），供下游速度闭环（PID）消费。
- 平滑：8 维恒定速度卡尔曼滤波（位置/尺寸 + 速度），抑制检测框逐帧抖动。
- Coast 预测：目标短暂消失时用滤波器预测值继续输出（`is_predicted=true`），
避免控制输出突变；连续丢失超过 `max_lost_frames`（严格大于）才判丢。
- ID 稳定：贪婪最邻近关联（中心距离 + IoU 加权代价）维持轨道 ID；
主目标锁定后不被高置信度新目标抢占，仅在轨道死亡或 `Reset()` 时解除。
**不做什么**（边界）：
- 不做光流估计、不做激光/惯导等多源融合（属 perception 模块职责）；
- 不做 Topic 订阅/发布接线、不依赖 spdlog/Eigen/OpenCV，零第三方依赖；
- 不做画面边界钳制与负尺寸约束（与技术路线 §4.5 注记保持一致）；
- 不要求检测置信度下限过滤（上游检测器已完成阈值过滤）。
## 二、接口与数据流
命名空间 `drone::tracker`，头文件保护 `#pragma once`。
| 头文件 | 内容 |
|--------|------|
| `include/target_tracker/kalman_box_filter.h` | `BoxCXCYWH`、`KalmanBoxFilter` |
| `include/target_tracker/tracklet.h` | `BoxXYXY`、`ToCenterSize`/`ToXYXY`/`ComputeIoU`、`Tracklet` |
| `include/target_tracker/target_tracker.h` | `Detection`、`FrameShape`、`TrackResult`、`TargetTracker` |
主要接口签名：
```cpp
class KalmanBoxFilter {
void Initiate(const BoxCXCYWH& z); // 首次观测初始化（§4.3）
std::optional<BoxCXCYWH> Predict(); // 未初始化返回 nullopt（§4.4）
void Update(const BoxCXCYWH& z); // 观测更新（§4.5）
bool Initialized() const;
const std::array<double, 8>& State() const; // [cx,cy,w,h,vx,vy,vw,vh]
};
struct Tracklet {
Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
int max_lost_frames);
bool IsDead() const; // lost_count > max_lost（严格大于）
bool IsCoasting() const; // lost_count > 0
void Predict(); // kf.predict 非空时更新 last_center/last_box、age+1
void Update(const BoxXYXY& box, double conf, int cls); // 匹配更新
void MarkMiss(); // lost+1、age+1
// 字段：track_id / kf / lost_count / max_lost / confidence / cls_id /
// age / hits / last_center / last_box / last_raw_box
};
class TargetTracker {
struct Config { // §8.1 默认值
int max_lost_frames = 8;
double max_association_dist = 200.0;
int min_hits = 3;
double dist_weight = 0.5;
double iou_weight = 0.5;
};
TargetTracker(); // 默认配置
explicit TargetTracker(const Config& cfg);
TrackResult Update(const std::vector<Detection>& detections,
std::optional<FrameShape> frame_shape = std::nullopt);
TrackResult LastResult() const; // 线程安全拷贝
void Reset(); // 清全部状态且 next_id 归零
int ActiveCount() const;
std::unordered_map<int, Tracklet> ActiveTracks() const; // 只读快照
};
```
数据流：调用方每帧将检测列表（像素坐标 `[x1,y1,x2,y2,conf,cls_id]`）传入
`Update()`，得到 `TrackResult`（九字段：tracked / primary_id / center / box /
confidence / is_predicted / lost_frames / n_active / raw）。丢失时
`tracked=false`、定位字段空、`lost_frames=0`（技术路线 §2.3）。
## 三、关键实现点
### 3.1 卡尔曼滤波（对照技术路线 §4）
- 状态 `x[8]=[cx,cy,w,h,vx,vy,vw,vh]`，观测 `z[4]=[cx,cy,w,h]`，dt=1 帧。
- 常量矩阵：`F`（8×8，I4 + 右上 I4）、`H`（4×8，[I4|0]）为 `constexpr`；
矩阵乘法/转置用 `std::array` 定长手写，全程 `double`（§9.3）。
- 常量：`kStdPos=1/50`、`kStdVel=1/200`、`kIniPos=2.0`、`kIniVel=100.0`、
`kRScale=2.0`（§4.2/§8.2）。
- **动态噪声**：`Predict()` 时 Q=diag(σ_pos²,σ_vel²)，σ 直接用状态中的 w/h
缩放；`Update()` 时 R 用 `abs(w)/abs(h)` 缩放并乘 2——两处差异与规格
§4.2 注记严格保持一致。
- `Initiate()`：速度置零，P=diag(2,2,2,2,100,100,100,100)（§4.3）。
- `Update()` 中 S=HPHᵀ+R 为 4×4，用**高斯-约当消元 + 部分主元**求逆，
消元前对角加 1e-9 正则化防奇异（§9.3）；随后 K=PHᵀS⁻¹、
x←x+K(z−Hx)、P←(I−KH)P（§4.5）。
### 3.2 贪婪关联（对照 §5）
- `cost = dist_weight·dist/max(Dmax,1e-3) + iou_weight·(1−IoU)`，
距离用 `std::hypot`；每条轨道 `best_cost` 初始化为 1.0，**仅 cost 严格
小于才匹配**（cost=1.0 恰好不匹配，§5.1）。
- 按轨道 confidence 降序（平局按 ID 升序保证确定性）逐一贪心匹配；
排序时复制 ID 到 `std::vector` 后 `std::sort`，不直接排哈希表（§9.2）。
### 3.3 主目标选举三级回退（对照 §6.3）
1. **锁定延续优先**：`locked_primary_id` 存活则直接输出（Coast 中照样输出
`is_predicted=true`），不被高置信度目标抢占；
2. **重新选举**（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits
的已确认池取 confidence 最大；池空则退回全部 measured；
3. 无 measured 时从 coasting 中选离画面中心最近者（欧氏距离平方，
`frame_shape` 缺省按 640×480 中心 (320,240)），`is_predicted=true`；
都没有则 `tracked=false`。
锁定仅在轨道死亡删除处或 `Reset()` 时解除。
### 3.4 主流程六步（对照 §7）
`Update()` 严格按：①全体轨道 predict ②检测中心表 + 贪婪关联 ③匹配轨道
update（raw_box 由检测重建）④未匹配轨道 mark_miss、死亡即删、若为锁定
目标同时清锁 ⑤未匹配检测新建轨道（创建当帧赋 last_box/last_raw_box，
保证当帧可参与输出；next_id 从 0 递增永不复用）⑥主目标选举组装输出并
缓存 last_result。
### 3.5 线程模型（对照 §9.5）
单线程写 `Update()`（推理主循环）；库内仅对 `last_result` 缓存加一把
`std::mutex`，`LastResult()` 返回受保护拷贝。内部轨道状态不加锁；
若未来改为多线程调用 `Update()` 需整体加锁。
### 3.6 符号导出说明
当前不加 `-fvisibility=hidden`、不使用导出宏，共享库默认导出全部公共符号。
日后若启用隐藏可见性（如 `CXX_VISIBILITY_PRESET hidden`），需补
`TARGET_TRACKER_API` 宏（导出/导入双态）并标注所有公共类与函数，
否则下游链接将缺失符号。
## 四、日志行为
本库为**纯算法层，不打任何日志**（不依赖 spdlog）。若未来接入主工程，
接线层（订阅/发布适配部件）按主工程日志纪律负责关键路径日志与节流，
本库内部不新增日志。
## 五、测试方式
构建环境：WSL2 Ubuntu（AGENTS.md 规定的开发机构建环境）。
```bash
cd /mnt/d/ProgramData/drone_test/drone_test
cmake -S target_tracker -B target_tracker/build
cmake --build target_tracker/build -j$(nproc)
ctest --test-dir target_tracker/build --output-on-failure
```
测试共 27 条（kalman_box_filter_test 5 条 / tracklet_test 12 条 /
target_tracker_test 10 条），其中 `target_tracker_test.cpp` 逐条覆盖
技术路线 §9.4 六条验收用例：
1. `EmptyFramesCoastThenLost`：空检测帧 Coast 持续 max_lost_frames 帧
（is_predicted=true），第 max_lost_frames+1 帧 tracked=false；
2. `IdStrictlyIncreasingNoReuse`：ID 严格递增不复用，Reset 后归零；
3. `LockNotStolenAndSwitchAfterDeath`：Coast 期间高置信度新目标不抢占锁定，
锁定目标死亡后切换；
4. `MinHitsFallbackAllowsNewTrack`：无已确认实测轨道时新轨道（hits<min_hits）
可当选主目标；
5. `CostExactlyOneDoesNotMatch`：纯距离项构造 cost=1.0 恰好不匹配，
并附 cost<1.0 对照帧；
6. `HigherConfidenceTrackWinsDetection`：两轨道竞争同一检测，置信度高者获胜。
其余为补充行为测试（丢失输出字段约定、Coast 兜底选举、锁定延续、
默认画面中心、IoU 边界、滤波器收敛性等）。
## 六、排查 / 修改要点
- **主目标"甩头"**：确认是否误改锁定解除逻辑——锁定只能在轨道死亡删除处
或 `Reset()` 解除；选举分支不得覆盖存活锁定。
- **Coast 时间不对**：死亡判定为 `lost_count > max_lost`（严格大于），
默认允许 Coast 8 帧、第 9 帧判丢；改成 `>=` 属于行为变更。
- **关联不上 / 乱匹配**：检查 `max_association_dist` 与目标帧间位移是否
匹配；cost 判据为严格小于 1.0；dist/iou 权重之和建议保持 1.0。
- **滤波发散或跳变**：检查 Q/R 动态噪声是否按状态 w/h 缩放（R 用 abs 且
乘 2、Q 不用 abs，该差异是规格要求）；4×4 求逆已加 1e-9 正则化。
- **测试链接失败（找不到 so）**：测试可执行文件通过 `CMAKE_BUILD_RPATH`
定位库；若把测试挪到别处运行需设置 `LD_LIBRARY_PATH`。
- 修改任何行为后必须同步更新本文档与 `docs/视觉追踪技术路线.md` 的对照注释。
## 七、未来接线指引（接入主工程视频链路）
本库以独立动态库交付，主工程接入建议采用：
1. **订阅解码帧主题作为每帧节拍**：接线部件订阅解码帧 Topic，以解码帧
到达为帧节拍驱动追踪（保证与推理节奏一致）；
2. **按 frame_sequence 聚合 DetectionResult**：YOLO 每目标发布一条
DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence
聚合成 `std::vector<Detection>` 后调用 `Update()`；
3. **主工程 `include/common/types.h` 新增 `TrackedTarget` 消息与
`kTrackedTarget` 主题**：由接线部件把 `TrackResult` 转换为
`TrackedTarget` 发布，供控制线程（20Hz PID）订阅消费；Coast
（is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车
（技术路线 §1.2 上游行为约定）。
主工程链接该 so 的 CMake 片段示例：
```cmake
# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程）
add_subdirectory(target_tracker)
target_link_libraries(drone_control PRIVATE target_tracker)
# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录
# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致）
```
reference-c++/target_tracker/tests/kalman_box_filter_test.cpp
+124
View File
@@ -0,0 +1,124 @@
// =============================================================================
// kalman_box_filter_test.cpp —— KalmanBoxFilter 单元测试
//
// 覆盖 docs/视觉追踪技术路线.md §4 行为：
// - 初始化状态正确（§4.3）
// - 静止目标多帧收敛到观测（§4.5）
// - 匀速目标速度收敛且预测超前一个 dt（§4.4）
// - 未初始化 predict 返回 nullopt（§4.4）
// =============================================================================
#include <gtest/gtest.h>
#include <array>
#include <cmath>
#include "target_tracker/kalman_box_filter.h"
using drone::tracker::BoxCXCYWH;
using drone::tracker::KalmanBoxFilter;
namespace {
// 未初始化时 predict 返回 nullopt（§4.4）；update 直接返回不改变状态（§4.5）
TEST(KalmanBoxFilterTest, UninitializedPredictReturnsNullopt) {
KalmanBoxFilter kf;
EXPECT_FALSE(kf.Initialized());
EXPECT_FALSE(kf.Predict().has_value());
// 未初始化时 update 无副作用
kf.Update({100.0, 100.0, 20.0, 20.0});
EXPECT_FALSE(kf.Initialized());
EXPECT_FALSE(kf.Predict().has_value());
for (const double v : kf.State()) {
EXPECT_DOUBLE_EQ(v, 0.0);
}
}
// 初始化状态正确（§4.3）：速度置零、位置/尺寸取首次观测
TEST(KalmanBoxFilterTest, InitiateSetsStateCorrectly) {
KalmanBoxFilter kf;
kf.Initiate({320.0, 240.0, 40.0, 30.0});
EXPECT_TRUE(kf.Initialized());
const std::array<double, 8> expected = {
{320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0}};
for (size_t i = 0; i < expected.size(); ++i) {
EXPECT_DOUBLE_EQ(kf.State()[i], expected[i]) << "index=" << i;
}
// 初始化后 predict 返回状态前 4 维副本（速度为零 → 位置不变，§4.4）
const auto pred = kf.Predict();
ASSERT_TRUE(pred.has_value());
EXPECT_NEAR(pred->cx, 320.0, 1e-9);
EXPECT_NEAR(pred->cy, 240.0, 1e-9);
EXPECT_NEAR(pred->w, 40.0, 1e-9);
EXPECT_NEAR(pred->h, 30.0, 1e-9);
}
// 静止目标：多帧重复观测同一框，滤波输出应收敛到观测值（§4.5）
TEST(KalmanBoxFilterTest, StaticTargetConvergesToObservation) {
KalmanBoxFilter kf;
const BoxCXCYWH z{100.0, 150.0, 40.0, 60.0};
kf.Initiate({104.0, 156.0, 42.0, 62.0}); // 带初始偏差，验证收敛能力
for (int i = 0; i < 50; ++i) {
kf.Predict();
kf.Update(z);
}
EXPECT_NEAR(kf.State()[0], z.cx, 0.5); // cx
EXPECT_NEAR(kf.State()[1], z.cy, 0.5); // cy
EXPECT_NEAR(kf.State()[2], z.w, 0.5); // w
EXPECT_NEAR(kf.State()[3], z.h, 0.5); // h
// 静止目标速度应收敛到约 0
EXPECT_NEAR(kf.State()[4], 0.0, 0.1);
EXPECT_NEAR(kf.State()[5], 0.0, 0.1);
}
// 匀速目标：速度估计收敛到真实速度，且 predict 超前一个 dt（§4.4）
TEST(KalmanBoxFilterTest, ConstantVelocityConvergesAndPredictLeadsOneDt) {
KalmanBoxFilter kf;
const double vx = 3.0; // 像素/帧
const double vy = 2.0; // 像素/帧
// 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
BoxCXCYWH z{100.0, 100.0, 30.0, 30.0};
kf.Initiate(z);
for (int i = 0; i < 60; ++i) {
kf.Predict();
z.cx += vx;
z.cy += vy;
kf.Update(z);
}
// 速度收敛到真实速度（像素/帧）
EXPECT_NEAR(kf.State()[4], vx, 0.3) << "vx 未收敛";
EXPECT_NEAR(kf.State()[5], vy, 0.3) << "vy 未收敛";
// 位置贴合当前观测
EXPECT_NEAR(kf.State()[0], z.cx, 1.5);
EXPECT_NEAR(kf.State()[1], z.cy, 1.5);
// 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
const auto pred = kf.Predict();
ASSERT_TRUE(pred.has_value());
EXPECT_NEAR(pred->cx, z.cx + vx, 1.0) << "预测未超前一个 dt";
EXPECT_NEAR(pred->cy, z.cy + vy, 1.0) << "预测未超前一个 dt";
}
// 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
TEST(KalmanBoxFilterTest, SmallTargetNoiseScalingStable) {
KalmanBoxFilter kf;
const BoxCXCYWH z{50.0, 60.0, 4.0, 4.0}; // 小目标（4×4 像素）
kf.Initiate(z);
for (int i = 0; i < 30; ++i) {
kf.Predict();
kf.Update(z);
}
EXPECT_NEAR(kf.State()[0], z.cx, 0.5);
EXPECT_NEAR(kf.State()[1], z.cy, 0.5);
EXPECT_TRUE(std::isfinite(kf.State()[2]));
EXPECT_TRUE(std::isfinite(kf.State()[3]));
}
} // namespace
reference-c++/target_tracker/tests/target_tracker_test.cpp
+402
View File
@@ -0,0 +1,402 @@
// =============================================================================
// target_tracker_test.cpp —— TargetTracker 单元测试
//
// 逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
// 1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
// 2. ID 严格递增不复用（含 Reset 后归零）
// 3. 锁定不被抢占；锁定目标死亡后切换到新目标
// 4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
// 5. cost=1.0 恰好不构成匹配（严格小于）
// 6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
// =============================================================================
#include <gtest/gtest.h>
#include <cmath>
#include <limits>
#include <optional>
#include <vector>
#include "target_tracker/target_tracker.h"
using drone::tracker::Detection;
using drone::tracker::FrameShape;
using drone::tracker::TargetTracker;
using drone::tracker::TrackResult;
namespace {
Detection MakeDet(double x1, double y1, double x2, double y2, double conf,
int cls = 0) {
return Detection{x1, y1, x2, y2, conf, cls};
}
// ============================================================================
// 验收用例 1（§9.4-1）：空检测帧序列
// 连续输入空检测，主目标输出 is_predicted=true 共 max_lost_frames 帧，
// 第 max_lost_frames + 1 帧 tracked=false（死亡判定 lost_count > max_lost）。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, EmptyFramesCoastThenLost) {
TargetTracker tracker; // 默认配置 max_lost_frames = 8
ASSERT_EQ(tracker.GetConfig().max_lost_frames, 8);
// 第 1 帧：给出一个检测，建立轨道并锁定为主目标
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
ASSERT_TRUE(r.tracked);
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_FALSE(r.is_predicted); // 当帧有实测
EXPECT_EQ(r.lost_frames, 0);
EXPECT_EQ(r.n_active, 1);
ASSERT_TRUE(r.box.has_value());
ASSERT_TRUE(r.raw.has_value()); // 当帧关联到原始检测框
// 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=true）
for (int i = 1; i <= 8; ++i) {
r = tracker.Update({});
ASSERT_TRUE(r.tracked) << "第 " << i << " 帧 Coast 期间不应判丢";
EXPECT_TRUE(r.is_predicted) << "第 " << i << " 帧应为纯预测输出";
EXPECT_EQ(r.lost_frames, i);
EXPECT_TRUE(r.primary_id.has_value());
EXPECT_TRUE(r.box.has_value()); // Coast 期间继续输出预测框
EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3）
EXPECT_DOUBLE_EQ(r.confidence, 0.9); // 继承最近实测置信度（§2.3）
}
// 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=false
r = tracker.Update({});
EXPECT_FALSE(r.tracked);
EXPECT_FALSE(r.primary_id.has_value());
EXPECT_FALSE(r.box.has_value());
EXPECT_EQ(r.lost_frames, 0); // 丢失时 lost_frames=0（§2.3）
EXPECT_EQ(r.n_active, 0);
}
// ============================================================================
// 验收用例 2（§9.4-2）：ID 单调性
// 多次进出画面的目标获得严格递增、不复用的 ID；Reset 后计数器归零。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, IdStrictlyIncreasingNoReuse) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 1; // 缩短 Coast 窗口，加速轨道死亡
TargetTracker tracker(cfg);
// 目标 A 出现 → id 0
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_a = *r.primary_id;
EXPECT_EQ(id_a, 0);
// 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
tracker.Update({});
r = tracker.Update({});
EXPECT_FALSE(r.tracked);
EXPECT_EQ(tracker.ActiveCount(), 0);
// 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_b = *r.primary_id;
EXPECT_EQ(id_b, 1);
EXPECT_GT(id_b, id_a);
// 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9),
MakeDet(400.0, 300.0, 430.0, 330.0, 0.8)});
EXPECT_EQ(tracker.ActiveCount(), 2);
int max_id = 0;
for (const auto& kv : tracker.ActiveTracks()) {
max_id = std::max(max_id, kv.first);
}
EXPECT_EQ(max_id, 2);
// Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
tracker.Reset();
EXPECT_EQ(tracker.ActiveCount(), 0);
EXPECT_FALSE(tracker.LastResult().tracked);
r = tracker.Update({MakeDet(50.0, 50.0, 70.0, 70.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, 0);
}
// ============================================================================
// 验收用例 3（§9.4-3）：锁定不被抢占
// 锁定目标 Coast 期间出现更高置信度新目标，primary_id 不变；
// 锁定目标死亡后切换到新目标。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, LockNotStolenAndSwitchAfterDeath) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 3;
TargetTracker tracker(cfg);
// 低置信度目标 A 建立并锁定
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.4)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_a = *r.primary_id;
// A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, id_a) << "Coast 期间锁定不应被高置信度目标抢占";
EXPECT_TRUE(r.is_predicted);
EXPECT_EQ(tracker.ActiveCount(), 2); // A（Coast）与 B 并存
// 继续 Coast，锁定依旧延续
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
EXPECT_EQ(*r.primary_id, id_a);
EXPECT_TRUE(r.is_predicted);
// A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
for (int i = 0; i < 2; ++i) {
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
}
// A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
ASSERT_TRUE(r.tracked);
EXPECT_NE(*r.primary_id, id_a) << "锁定目标死亡后应切换到新目标";
EXPECT_FALSE(r.is_predicted);
// 后续 B 持续作为主目标
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_NE(*r.primary_id, id_a);
}
// ============================================================================
// 验收用例 4（§9.4-4）：min_hits 回退
// 新轨道（hits 未达 min_hits）在无其他已确认实测轨道时也可当选主目标。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, MinHitsFallbackAllowsNewTrack) {
TargetTracker::Config cfg;
cfg.min_hits = 5; // 抬高确认门槛，保证测试期间轨道始终未确认
TargetTracker tracker(cfg);
// 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
TrackResult r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
ASSERT_TRUE(r.tracked) << "无已确认轨道时新轨道应能当选主目标";
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, 0);
EXPECT_FALSE(r.is_predicted);
// 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
for (int i = 0; i < 3; ++i) {
r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
ASSERT_TRUE(r.tracked);
EXPECT_EQ(*r.primary_id, 0);
}
}
// ============================================================================
// 验收用例 5（§9.4-5）：cost 边界
// 恰好 cost = 1.0 不构成匹配（仅严格小于才匹配）。
// 构造：w_dist=1、w_iou=0（纯距离项），中心距离恰等于 Dmax → cost=1.0；
// 同时给出略小于阈值的对照帧证明边界两侧行为正确。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, CostExactlyOneDoesNotMatch) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 1;
cfg.max_association_dist = 200.0;
cfg.dist_weight = 1.0; // 纯距离项，IoU 项权重为 0 且两框零交集
cfg.iou_weight = 0.0;
TargetTracker tracker(cfg);
// 建立轨道：中心 (100,100)
TrackResult r = tracker.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_a = *r.primary_id;
// 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
// （预测后轨道中心速度为 0，仍位于 (100,100)；两框零交集 IoU=0 无影响）
r = tracker.Update({MakeDet(90.0, 290.0, 110.0, 310.0, 0.9)});
// 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
EXPECT_EQ(tracker.ActiveCount(), 2) << "cost=1.0 不应匹配，应新建轨道";
// 原轨道（锁定）进入 Coast：lost_frames=1
EXPECT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_EQ(*r.primary_id, id_a);
EXPECT_EQ(r.lost_frames, 1);
// 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
TargetTracker tracker2(cfg);
r = tracker2.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
const int id_b = *r.primary_id;
r = tracker2.Update({MakeDet(90.0, 289.0, 110.0, 309.0, 0.9)});
EXPECT_EQ(tracker2.ActiveCount(), 1) << "cost<1.0 应匹配成功，不应新建轨道";
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, id_b);
EXPECT_FALSE(r.is_predicted); // 实测命中
}
// ============================================================================
// 验收用例 6（§9.4-6）：置信度优先
// 两轨道竞争同一检测时，置信度高者获胜。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, HigherConfidenceTrackWinsDetection) {
TargetTracker tracker;
// 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.5),
MakeDet(180.0, 100.0, 200.0, 120.0, 0.9)});
ASSERT_EQ(tracker.ActiveCount(), 2);
// 找出两条轨道 ID（按置信度区分）
int id_low = -1;
int id_high = -1;
for (const auto& kv : tracker.ActiveTracks()) {
if (kv.second.confidence > 0.8) {
id_high = kv.first;
} else {
id_low = kv.first;
}
}
ASSERT_NE(id_low, -1);
ASSERT_NE(id_high, -1);
// 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
// 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
r = tracker.Update({MakeDet(140.0, 100.0, 160.0, 120.0, 0.7)});
const auto tracks = tracker.ActiveTracks();
ASSERT_EQ(tracks.size(), 2u);
// 获胜者：命中（lost=0、hits=2、置信度更新）
EXPECT_EQ(tracks.at(id_high).lost_count, 0) << "高置信度轨道应获胜";
EXPECT_EQ(tracks.at(id_high).hits, 2);
EXPECT_DOUBLE_EQ(tracks.at(id_high).confidence, 0.7);
// 失败者：未匹配进入 Coast（lost=1、hits 不变）
EXPECT_EQ(tracks.at(id_low).lost_count, 1) << "低置信度轨道应落选";
EXPECT_EQ(tracks.at(id_low).hits, 1);
EXPECT_DOUBLE_EQ(tracks.at(id_low).confidence, 0.5);
}
// ============================================================================
// NaN/Inf 防御（评审修复项）：坐标或置信度含非有限值的检测被丢弃，
// 不崩溃、不新建轨道、不污染主目标输出
// ============================================================================
TEST(TargetTrackerAcceptanceTest, NonFiniteDetectionsAreDiscarded) {
TargetTracker tracker;
const double nan = std::numeric_limits<double>::quiet_NaN();
const double inf = std::numeric_limits<double>::infinity();
// 第 1 帧：建立正常轨道
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
ASSERT_TRUE(r.tracked);
// 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
// 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
r = tracker.Update({MakeDet(nan, 100.0, 140.0, 140.0, 0.9),
MakeDet(100.0, -inf, 140.0, 140.0, 0.9),
MakeDet(100.0, 100.0, nan, 140.0, 0.9),
MakeDet(102.0, 102.0, 142.0, 142.0, nan),
MakeDet(101.0, 101.0, 141.0, 141.0, 0.9)});
EXPECT_EQ(tracker.ActiveCount(), 1) << "非有限检测不应新建轨道";
ASSERT_TRUE(r.tracked);
EXPECT_FALSE(r.is_predicted); // 正常检测命中，实测输出
// 主目标输出未被 NaN 污染
EXPECT_TRUE(std::isfinite(r.center.first));
EXPECT_TRUE(std::isfinite(r.center.second));
ASSERT_TRUE(r.box.has_value());
EXPECT_TRUE(std::isfinite(r.box->x1) && std::isfinite(r.box->y1));
EXPECT_TRUE(std::isfinite(r.box->x2) && std::isfinite(r.box->y2));
EXPECT_TRUE(std::isfinite(r.confidence));
ASSERT_TRUE(r.raw.has_value()); // 实测帧 raw 有值
// 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
r = tracker.Update({MakeDet(nan, nan, nan, nan, nan)});
EXPECT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_EQ(r.lost_frames, 1);
EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3）
EXPECT_TRUE(std::isfinite(r.center.first));
// 持续 NaN 检测直至轨道死亡回收，全程不崩溃
for (int i = 0; i < 10; ++i) {
r = tracker.Update({MakeDet(nan, 0.0, 10.0, 10.0, 0.9)});
}
EXPECT_FALSE(r.tracked);
EXPECT_EQ(tracker.ActiveCount(), 0);
}
// ============================================================================
// 补充行为测试（§2.3 / §6.3 / §9.5）
// ============================================================================
// 丢失时返回值约定：tracked=false、定位字段空、lost_frames=0（§2.3）
TEST(TargetTrackerBehaviorTest, LostResultFieldsAreEmpty) {
TargetTracker tracker;
TrackResult r = tracker.Update({}); // 空帧序列起点：无任何轨道
EXPECT_FALSE(r.tracked);
EXPECT_FALSE(r.primary_id.has_value());
EXPECT_FALSE(r.box.has_value());
EXPECT_FALSE(r.raw.has_value());
EXPECT_EQ(r.lost_frames, 0);
EXPECT_EQ(r.n_active, 0);
EXPECT_DOUBLE_EQ(r.confidence, 0.0);
// LastResult 返回同一快照（§9.5 跨线程读取入口）
const TrackResult cached = tracker.LastResult();
EXPECT_FALSE(cached.tracked);
}
// Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
// 注意锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支，
// 故采用错帧创建 + 缩短 max_lost 的方式构造场景。
TEST(TargetTrackerBehaviorTest, CoastingFallbackChoosesNearestToFrameCenter) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 2;
TargetTracker tracker(cfg);
// F1：角落目标（高置信度）先建立并被锁定
tracker.Update({MakeDet(10.0, 10.0, 30.0, 30.0, 0.95)});
// F2：画面中心附近目标建立；角落目标丢失(1)
tracker.Update({MakeDet(300.0, 220.0, 340.0, 260.0, 0.9)});
// F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
tracker.Update({MakeDet(600.0, 450.0, 620.0, 470.0, 0.85)});
// F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
// 其余两条 Coast 存活 → 重新选举选离画面中心最近者
TrackResult r = tracker.Update({}, FrameShape{480, 640});
ASSERT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_NEAR(r.center.first, 320.0, 25.0); // 靠近画面中心 (320,240) 者当选
EXPECT_NEAR(r.center.second, 240.0, 25.0);
EXPECT_EQ(r.n_active, 2); // 角落轨道已回收
}
// 锁定延续优先于置信度选举：锁定轨道实测时即使置信度较低也保持主目标（§6.3 ①）
TEST(TargetTrackerBehaviorTest, LockedPrimaryKeptWhileMeasured) {
TargetTracker tracker;
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.4)});
ASSERT_TRUE(r.primary_id.has_value());
const int locked_id = *r.primary_id;
// 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
for (int i = 0; i < 5; ++i) {
r = tracker.Update({MakeDet(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
MakeDet(400.0, 300.0, 430.0, 330.0, 0.95)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, locked_id);
EXPECT_FALSE(r.is_predicted);
}
}
// frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）：
// 两条 Coast 轨道分别位于默认中心两侧，当选者应为更近的一侧
TEST(TargetTrackerBehaviorTest, DefaultFrameCenterIs320x240) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 2;
TargetTracker tracker(cfg);
// F1：角落目标先建立并被锁定（高置信度）
tracker.Update({MakeDet(0.0, 0.0, 20.0, 20.0, 0.95)});
// F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
tracker.Update({MakeDet(270.0, 230.0, 290.0, 250.0, 0.9)});
// F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
tracker.Update({MakeDet(490.0, 230.0, 510.0, 250.0, 0.85)});
// F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
// （不传 frame_shape → 按默认画面中心 (320,240)；两候选间距足够大，
// 速度估计漂移不影响最近者判定）
TrackResult r = tracker.Update({});
ASSERT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_NEAR(r.center.first, 280.0, 30.0); // 左侧更近，当选
EXPECT_NEAR(r.center.second, 240.0, 30.0);
}
} // namespace
reference-c++/target_tracker/tests/tracklet_test.cpp
+177
View File
@@ -0,0 +1,177 @@
// =============================================================================
// tracklet_test.cpp —— Tracklet 与几何工具单元测试
//
// 覆盖 docs/视觉追踪技术路线.md：
// - IoU 边界（空框 / 零交集 / 完全重合=1，§5.2）
// - IsDead / IsCoasting 判定（§3.2 派生状态，§6.1 严格大于）
// - Predict / Update / MarkMiss 计数语义（§7 末尾伪代码）
// - BoxXYXY ↔ BoxCXCYWH 互转（§7）
// =============================================================================
#include <gtest/gtest.h>
#include "target_tracker/tracklet.h"
using drone::tracker::BoxCXCYWH;
using drone::tracker::BoxXYXY;
using drone::tracker::ComputeIoU;
using drone::tracker::ToCenterSize;
using drone::tracker::ToXYXY;
using drone::tracker::Tracklet;
namespace {
// ---- 框表示互转（§7） ----
TEST(BoxConversionTest, RoundTripPreservesValues) {
const BoxXYXY box{10.0, 20.0, 50.0, 80.0};
const BoxCXCYWH c = ToCenterSize(box);
EXPECT_DOUBLE_EQ(c.cx, 30.0); // (10+50)/2
EXPECT_DOUBLE_EQ(c.cy, 50.0); // (20+80)/2
EXPECT_DOUBLE_EQ(c.w, 40.0); // 50−10
EXPECT_DOUBLE_EQ(c.h, 60.0); // 80−20
const BoxXYXY back = ToXYXY(c);
EXPECT_DOUBLE_EQ(back.x1, box.x1);
EXPECT_DOUBLE_EQ(back.y1, box.y1);
EXPECT_DOUBLE_EQ(back.x2, box.x2);
EXPECT_DOUBLE_EQ(back.y2, box.y2);
}
// ---- IoU 边界（§5.2） ----
TEST(IoUTest, IdenticalBoxesReturnOne) {
const BoxXYXY a{10.0, 10.0, 30.0, 30.0};
EXPECT_NEAR(ComputeIoU(a, a), 1.0, 1e-12); // 完全重合 = 1
}
TEST(IoUTest, DisjointBoxesReturnZero) {
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
const BoxXYXY b{20.0, 20.0, 30.0, 30.0};
EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0); // 零交集
}
TEST(IoUTest, TouchingEdgesReturnZero) {
// 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
const BoxXYXY b{10.0, 0.0, 20.0, 10.0}; // 右边缘相接
EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0);
const BoxXYXY c{10.0, 10.0, 20.0, 20.0}; // 角点相接
EXPECT_DOUBLE_EQ(ComputeIoU(a, c), 0.0);
}
TEST(IoUTest, EmptyBoxReturnsZero) {
const BoxXYXY normal{0.0, 0.0, 10.0, 10.0};
const BoxXYXY zero_area{5.0, 5.0, 5.0, 5.0}; // 零面积
const BoxXYXY inverted{10.0, 10.0, 0.0, 0.0}; // 反向（负宽高）
EXPECT_DOUBLE_EQ(ComputeIoU(normal, zero_area), 0.0);
EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, normal), 0.0);
EXPECT_DOUBLE_EQ(ComputeIoU(normal, inverted), 0.0);
EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, zero_area), 0.0);
}
TEST(IoUTest, PartialOverlapIsCorrect) {
// A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
const BoxXYXY b{5.0, 5.0, 15.0, 15.0};
EXPECT_NEAR(ComputeIoU(a, b), 25.0 / 175.0, 1e-12);
EXPECT_NEAR(ComputeIoU(b, a), 25.0 / 175.0, 1e-12); // 对称性
}
// ---- 构造与派生状态（§3.2 / §6.1） ----
TEST(TrackletTest, ConstructionInitialFields) {
const BoxCXCYWH meas{100.0, 100.0, 20.0, 20.0};
Tracklet t(7, meas, 0.9, 1, 8);
EXPECT_EQ(t.track_id, 7);
EXPECT_EQ(t.lost_count, 0);
EXPECT_EQ(t.max_lost, 8);
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
EXPECT_EQ(t.cls_id, 1);
EXPECT_EQ(t.age, 1); // 创建时为 1（§3.2）
EXPECT_EQ(t.hits, 1); // 创建时为 1（§3.2）
EXPECT_DOUBLE_EQ(t.last_center.first, 100.0);
EXPECT_DOUBLE_EQ(t.last_center.second, 100.0);
ASSERT_TRUE(t.last_box.has_value()); // 创建当帧即赋 last_box（§7 步骤 5）
EXPECT_DOUBLE_EQ(t.last_box->x1, 90.0);
EXPECT_DOUBLE_EQ(t.last_box->y1, 90.0);
EXPECT_DOUBLE_EQ(t.last_box->x2, 110.0);
EXPECT_DOUBLE_EQ(t.last_box->y2, 110.0);
EXPECT_FALSE(t.IsDead());
EXPECT_FALSE(t.IsCoasting());
}
// IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
TEST(TrackletTest, IsDeadUsesStrictlyGreater) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
for (int i = 0; i < 8; ++i) {
t.MarkMiss();
EXPECT_FALSE(t.IsDead()) << "lost_count=" << t.lost_count
<< " 未超过 max_lost 不应判死";
}
EXPECT_EQ(t.lost_count, 8);
t.MarkMiss(); // lost_count = 9 > 8
EXPECT_TRUE(t.IsDead());
}
TEST(TrackletTest, IsCoastingWhenLostPositive) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
EXPECT_FALSE(t.IsCoasting());
t.MarkMiss();
EXPECT_TRUE(t.IsCoasting()); // lost_count > 0 即 Coast（§3.2）
}
// ---- 生命周期方法计数语义（§7） ----
TEST(TrackletTest, PredictAdvancesAgeAndUpdatesLastState) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
const int age_before = t.age;
t.Predict(); // 速度为零：位置不变，但 age+1、last_box 更新为预测框
EXPECT_EQ(t.age, age_before + 1);
ASSERT_TRUE(t.last_box.has_value());
EXPECT_NEAR(t.last_center.first, 100.0, 1e-9);
EXPECT_NEAR(t.last_center.second, 100.0, 1e-9);
// hits / lost_count / confidence 不受预测影响
EXPECT_EQ(t.hits, 1);
EXPECT_EQ(t.lost_count, 0);
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
}
TEST(TrackletTest, UpdateResetsLostAndIncrementsHits) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
t.MarkMiss();
t.MarkMiss();
ASSERT_EQ(t.lost_count, 2);
ASSERT_TRUE(t.IsCoasting());
const int age_before = t.age;
const BoxXYXY box{150.0, 160.0, 170.0, 180.0};
t.Update(box, 0.75, 2);
EXPECT_EQ(t.lost_count, 0); // 匹配成功清零（§7）
EXPECT_EQ(t.hits, 2); // hits+1
EXPECT_EQ(t.age, age_before + 1); // age+1
EXPECT_DOUBLE_EQ(t.confidence, 0.75);
EXPECT_EQ(t.cls_id, 2);
EXPECT_DOUBLE_EQ(t.last_center.first, 160.0); // (150+170)/2
EXPECT_DOUBLE_EQ(t.last_center.second, 170.0); // (160+180)/2
ASSERT_TRUE(t.last_box.has_value());
EXPECT_DOUBLE_EQ(t.last_box->x1, box.x1);
ASSERT_TRUE(t.last_raw_box.has_value()); // 实测框同步记录
EXPECT_DOUBLE_EQ(t.last_raw_box->x2, box.x2);
EXPECT_FALSE(t.IsCoasting());
}
TEST(TrackletTest, MarkMissIncrementsLostAndAge) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
const int age_before = t.age;
t.MarkMiss();
EXPECT_EQ(t.lost_count, 1);
EXPECT_EQ(t.age, age_before + 1);
// MarkMiss 不影响 hits / confidence
EXPECT_EQ(t.hits, 1);
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
}
} // namespace
reference-c++/visual_track_control/CMakeLists.txt
+122
View File
@@ -0,0 +1,122 @@
cmake_minimum_required(VERSION 3.16)
# =============================================================================
# visual_track_control —— 视觉跟踪控制律静态库（独立子工程）
#
# 实现 docs/物理追踪思路.md 的水平航向控制（§4）、垂直/高度双支路控制（§5）、
# 距离 PID（§6）、全量限幅（§8）、Coast 清积分与判丢刹车（§6.4）、
# 超龄降级（§7.1、§9.2）。
#
# 构建约束：
# - 与主工程完全解耦：根 CMakeLists.txt 零改动，可独立构建；
# - 仅依赖 third_party：nlohmann（单头文件）、spdlog（源码）、
# c_library_v2-master（MAVLink 纯头文件）；
# - target_tracker 为 SHARED 库且零源码修改，原样 add_subdirectory 复用，
# TrackResult 出现在本库公共头（vtc_types.h），故以 PUBLIC 链接；
# - MAVLink include 目录设为 SYSTEM PRIVATE：压告警噪音且不泄漏给下游，
# 全工程仅 src/mavlink_command_gateway.cpp 一个 TU 包含 mavlink.h
# （MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义）。
# =============================================================================
project(visual_track_control LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# target_tracker 是 SHARED 库：BUILD_RPATH 指向构建目录，示例无需
# LD_LIBRARY_PATH 即可直接运行（部署时需随带 libtarget_tracker.so.1）。
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};${CMAKE_BINARY_DIR}/target_tracker;$ORIGIN")
# ---- 依赖：target_tracker（零改动复用；其测试块由 -DBUILD_TESTING=OFF 压制） ----
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../target_tracker
${CMAKE_BINARY_DIR}/target_tracker)
# ---- 依赖：spdlog（守卫引入，避免挂载进根构建时重复定义；关闭示例/测试/动态库/安装） ----
if(NOT TARGET spdlog::spdlog)
set(SPDLOG_BUILD_EXAMPLE OFF CACHE BOOL "" FORCE)
set(SPDLOG_BUILD_EXAMPLE_HO OFF CACHE BOOL "" FORCE)
set(SPDLOG_BUILD_TESTS OFF CACHE BOOL "" FORCE)
set(SPDLOG_BUILD_TESTS_HO OFF CACHE BOOL "" FORCE)
set(SPDLOG_BUILD_SHARED OFF CACHE BOOL "" FORCE)
set(SPDLOG_INSTALL OFF CACHE BOOL "" FORCE)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../third_party/spdlog
${CMAKE_BINARY_DIR}/spdlog)
endif()
# ---- 静态库本体 ----
add_library(visual_track_control STATIC
src/vtc_config.cpp
src/pid_controller.cpp
src/line_of_sight.cpp
src/tracking_control_law.cpp
src/mavlink_command_gateway.cpp)
target_include_directories(visual_track_control PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
# nlohmann JSON：单头文件，仅配置加载器（启动期）使用，不进入热路径
target_include_directories(visual_track_control PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/../third_party/nlohmann)
# MAVLink 纯头文件库：SYSTEM 压告警、PRIVATE 不泄漏（公共头无任何 mavlink_* 类型）
target_include_directories(visual_track_control SYSTEM PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/../third_party/c_library_v2-master)
target_link_libraries(visual_track_control
PUBLIC target_tracker
PRIVATE spdlog::spdlog)
target_compile_features(visual_track_control PUBLIC cxx_std_17)
# 编译告警：与主工程一致
if(MSVC)
target_compile_options(visual_track_control PRIVATE /W4 /permissive-)
else()
target_compile_options(visual_track_control PRIVATE -Wall -Wextra -Wpedantic)
endif()
# ---- 示例可执行文件（合成检测 → 追踪 → 控制律 → MAVLink 字节流打印） ----
add_executable(track_control_demo examples/track_control_demo.cpp)
target_link_libraries(track_control_demo PRIVATE visual_track_control spdlog::spdlog)
target_compile_definitions(track_control_demo PRIVATE
VTC_DEMO_DEFAULT_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json")
if(MSVC)
target_compile_options(track_control_demo PRIVATE /W4 /permissive-)
else()
target_compile_options(track_control_demo PRIVATE -Wall -Wextra -Wpedantic)
endif()
# ---- 单元测试（Google Test） ----
# 独立选项：构建本子工程时需 -DBUILD_TESTING=OFF 压制 target_tracker 的测试，
# 故本库测试开关不复用 BUILD_TESTING，二者互不干扰。
# 开发机未安装 GTest 时自动跳过（不影响库与示例构建）。
option(VTC_BUILD_TESTS "构建 visual_track_control 单元测试" ON)
if(VTC_BUILD_TESTS)
find_package(GTest QUIET)
if(GTest_FOUND)
enable_testing()
include(GoogleTest)
add_executable(vtc_unit_tests
tests/rate_limiter_test.cpp
tests/staleness_checker_test.cpp
tests/frame_transform_test.cpp
tests/line_of_sight_test.cpp
tests/pid_controller_test.cpp
tests/tracking_control_law_test.cpp
tests/vtc_config_test.cpp
tests/mavlink_command_gateway_test.cpp)
target_link_libraries(vtc_unit_tests PRIVATE
visual_track_control GTest::gtest GTest::gtest_main)
target_compile_definitions(vtc_unit_tests PRIVATE
VTC_TEST_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json"
VTC_TEST_TMP_DIR="${CMAKE_CURRENT_BINARY_DIR}")
if(MSVC)
target_compile_options(vtc_unit_tests PRIVATE /W4 /permissive-)
else()
target_compile_options(vtc_unit_tests PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(vtc_unit_tests)
else()
message(STATUS "未找到 GTest：跳过 vtc_unit_tests 单元测试")
endif()
endif()
reference-c++/visual_track_control/config/track_control.json
+48
View File
@@ -0,0 +1,48 @@
{
"_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。",
"control": {
"frequency_hz": 20.0,
"visual_stale_ms": 200,
"radar_stale_ms": 500,
"attitude_stale_ms": 300
},
"camera": {
"image_width": 1920,
"image_height": 1080,
"fov_h_deg": 60.0,
"fov_v_deg": 35.0
},
"heading": {
"mode": "rate",
"gain": 1.0,
"yaw_rate_limit_dps": 30.0,
"yaw_slew_limit_dps2": 60.0
},
"vertical": {
"vz_gain_mps_per_deg": 0.1,
"vz_limit_mps": 2.0
},
"distance": {
"d_exp_m": 10.0,
"kp": 0.5,
"ki": 0.05,
"kd": 0.1,
"integral_limit": 2.0,
"derivative_filter_coef": 0.2,
"approach_velocity_limit_mps": 3.0,
"retreat_velocity_limit_mps": 1.0,
"no_distance_action": "hold",
"no_distance_approach_limit_mps": 0.5
},
"accel_limit": {
"ax_mps2": 2.0,
"ay_mps2": 2.0,
"az_mps2": 1.5
}
}
reference-c++/visual_track_control/examples/track_control_demo.cpp
+225
View File
@@ -0,0 +1,225 @@
// =============================================================================
// track_control_demo.cpp —— 视觉跟踪控制律演示程序
//
// 数据流（模拟单线程整合的完整链路）：
// 合成运动检测框 → TargetTracker::Update()（模拟推理线程每帧调用）
// → LastResult() + 打戳 → ControlSnapshot → TrackingControlLaw::Update()
// → MavlinkCommandGateway → LoggingTransport（字节数 + 十六进制摘要）
//
// 依次演示三种分支（对应物理追踪思路 §6.4）：
// 1. 正常跟踪：持续喂入合成检测框，输出速度航向意图（垂直/距离通道
// 统一为速度分量）；
// 2. Coast：停止喂检测 → is_predicted==true → 清积分，继续预测跟踪；
// 3. 判丢刹车：连续丢帧超过追踪库阈值 → tracked==false → 零速刹车。
//
// 运行方式：
// track_control_demo [配置文件路径] # 缺省使用编译期内置路径
// =============================================================================
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstdio>
#include <string>
#include <thread>
#include <vector>
#include <spdlog/spdlog.h>
#include <target_tracker/target_tracker.h>
#include "visual_track_control/i_command_transport.h"
#include "visual_track_control/mavlink_command_gateway.h"
#include "visual_track_control/tracking_control_law.h"
#include "visual_track_control/vtc_config.h"
#include "visual_track_control/vtc_types.h"
#ifndef VTC_DEMO_DEFAULT_CONFIG
#define VTC_DEMO_DEFAULT_CONFIG "config/track_control.json"
#endif
namespace {
// ---- 演示时长（秒）：总时长有限，自动退出 ----
constexpr int kNormalPhaseSeconds = 6; // 阶段1：正常跟踪
constexpr int kCoastPhaseSeconds = 4; // 阶段2：Coast（停止喂检测）
constexpr int kLostPhaseSeconds = 3; // 阶段3：判丢刹车
// 追踪库判丢阈值放大：默认 8 帧在 20Hz 下 Coast 仅 0.4 秒，不便于观察；
// 演示专用放宽到 60 帧（约 3 秒），正式参数以追踪库规格为准。
constexpr int kDemoMaxLostFrames = 60;
const char* ModeName(drone::vtc::ControlMode mode) {
switch (mode) {
case drone::vtc::ControlMode::kVelocityHeading: return "速度航向";
case drone::vtc::ControlMode::kBrakeHover: return "零速刹车";
case drone::vtc::ControlMode::kDegradedHold: return "降级保持";
}
return "未知";
}
/// 演示用传输实现：统计字节数并打印十六进制摘要（按秒级节流，防止刷屏）
class LoggingTransport : public drone::vtc::ICommandTransport {
public:
bool Send(const std::uint8_t* data, std::size_t len) override {
++msg_count_;
total_bytes_ += len;
const auto now_ms =
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::steady_clock::now().time_since_epoch())
.count();
if (now_ms - last_print_ms_ >= 1000) {
// 十六进制摘要：取包头前 12 字节
std::string hex;
char buf[4];
const std::size_t n = len < 12 ? len : 12;
for (std::size_t i = 0; i < n; ++i) {
std::snprintf(buf, sizeof(buf), "%02X ", data[i]);
hex += buf;
}
spdlog::info("[Transport] 累计 {} 帧 / {} 字节; 本帧 {} 字节, 包头: {}",
msg_count_, total_bytes_, len, hex);
last_print_ms_ = now_ms;
}
return true;
}
private:
std::int64_t last_print_ms_ = 0;
std::uint64_t msg_count_ = 0;
std::uint64_t total_bytes_ = 0;
};
/// 合成运动检测框：从画面右下向中心缓慢漂移（模拟目标被持续锁定）
std::vector<drone::tracker::Detection> SynthesizeDetections(double elapsed_s,
int width, int height) {
const double center_x = width * 0.5 + 160.0 - 20.0 * elapsed_s;
const double center_y = height * 0.5 + 80.0 - 10.0 * elapsed_s;
const double half_w = 70.0;
const double half_h = 50.0;
drone::tracker::Detection det;
det.x1 = center_x - half_w;
det.y1 = center_y - half_h;
det.x2 = center_x + half_w;
det.y2 = center_y + half_h;
det.conf = 0.9;
det.cls_id = 0;
return {det};
}
} // namespace
int main(int argc, char** argv) {
// ---- 启动期：一次性加载配置（缺字段/非法值直接抛异常终止） ----
const std::string config_path = argc > 1 ? argv[1] : VTC_DEMO_DEFAULT_CONFIG;
drone::vtc::TrackingConfig cfg;
try {
cfg = drone::vtc::TrackingConfig::LoadFromJson(config_path);
} catch (const std::exception& e) {
spdlog::error("配置加载失败: {}", e.what());
return 1;
}
spdlog::info("配置加载成功: {} (控制频率 {} Hz)", config_path,
cfg.control.frequency_hz);
// ---- 构造：追踪库 / 控制律 / 传输 / 网关 ----
drone::tracker::TargetTracker::Config tracker_cfg;
tracker_cfg.max_lost_frames = kDemoMaxLostFrames; // 演示专用判丢阈值
drone::tracker::TargetTracker tracker(tracker_cfg);
drone::vtc::TrackingControlLaw control_law(cfg);
LoggingTransport transport;
drone::vtc::MavlinkCommandGateway gateway(transport, /*sys=*/1, /*comp=*/1,
/*target_sys=*/1, /*target_comp=*/1);
const int width = cfg.camera.image_width;
const int height = cfg.camera.image_height;
const int period_ms = static_cast<int>(1000.0 / cfg.control.frequency_hz);
const int normal_ticks = kNormalPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
const int coast_ticks = kCoastPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
const int lost_ticks = kLostPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
const int total_ticks = normal_ticks + coast_ticks + lost_ticks;
spdlog::info("演示开始: 正常跟踪 {}s → Coast {}s → 判丢刹车 {}s (共 {} 拍, {} ms/拍)",
kNormalPhaseSeconds, kCoastPhaseSeconds, kLostPhaseSeconds,
total_ticks, period_ms);
drone::tracker::FrameShape frame_shape;
frame_shape.height = height;
frame_shape.width = width;
const char* current_phase = nullptr;
// ---- 主循环：按配置频率固定节拍（§7.1） ----
for (int tick = 0; tick < total_ticks; ++tick) {
const double elapsed_s = static_cast<double>(tick) / cfg.control.frequency_hz;
// 模拟单调时钟：1 秒偏移保证取样时间戳有效
const std::int64_t now_ms = 1000 + static_cast<std::int64_t>(tick) * period_ms;
// ---- 阶段划分 ----
const char* phase = tick < normal_ticks ? "正常跟踪"
: tick < normal_ticks + coast_ticks ? "Coast"
: "判丢刹车";
if (phase != current_phase) {
spdlog::info("===== 阶段切换: {} =====", phase);
current_phase = phase;
}
// ---- 模拟推理线程：仅正常跟踪阶段喂入检测框 ----
// 阶段2/3 停止喂检测：先 Coast（is_predicted），后判丢（tracked==false）
std::vector<drone::tracker::Detection> detections;
if (tick < normal_ticks) {
detections = SynthesizeDetections(elapsed_s, width, height);
}
tracker.Update(detections, frame_shape);
// ---- 控制线程：LastResult() + 打戳（TrackResult 无时间戳，§7.2） ----
drone::vtc::ControlSnapshot snapshot;
snapshot.track = tracker.LastResult();
snapshot.track_time_ms = now_ms;
// 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径）
snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s;
snapshot.associated_to_target = (tick < normal_ticks) &&
snapshot.radar_distance_m > 0.0;
snapshot.radar_time_ms = now_ms;
// 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot）
snapshot.attitude_present = true;
snapshot.attitude_time_ms = now_ms;
// ---- 控制律单周期计算 ----
const drone::vtc::ControlOutput output = control_law.Update(snapshot, now_ms);
// ---- 按模式经网关发送 ----
const auto time_boot_ms = static_cast<std::uint32_t>(now_ms);
switch (output.mode) {
case drone::vtc::ControlMode::kVelocityHeading:
gateway.SendVelocityHeading(output, time_boot_ms);
break;
case drone::vtc::ControlMode::kBrakeHover:
gateway.SendBrakeHover(time_boot_ms);
break;
case drone::vtc::ControlMode::kDegradedHold:
break; // 降级保持：不发送指令（仅告警，§7.1）
}
// ---- 半秒一次的摘要打印（演示观测用，非生产热路径） ----
if (tick % 10 == 0) {
spdlog::info(
"[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} "
"vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 关联={}",
elapsed_s, phase, ModeName(output.mode),
output.coast_active ? "(Coast)" : "", snapshot.track.tracked,
output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps,
snapshot.radar_distance_m, snapshot.associated_to_target);
}
std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
}
control_law.Reset(); // 停止视觉闭环：清全部状态（§6.4）
spdlog::info("演示结束");
return 0;
}
reference-c++/visual_track_control/include/visual_track_control/frame_transform.h
+44
View File
@@ -0,0 +1,44 @@
#pragma once
// =============================================================================
// frame_transform.h —— 机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转
//
// 对应规格：docs/物理追踪思路.md
// §2.3 控制律内部计算统一在机体坐标系；输出的速度意图若采用局部系，
// 必须随姿态完成旋转并显式标注坐标系，禁止混用未标注坐标系的数据。
//
// 实现要点：
// - 速度设定值只随**航向角 ψ**（绕下轴旋转）旋转：水平速度分量的
// 姿态耦合项由飞控内环处理，这是 PX4 offboard 速度设定值的标准做法；
// 滚转/俯仰不参与（对速度指令为二阶小量）；
// - 旋转为正交变换：合速度大小不变，因此"限幅在机体系完成、旋转在
// 输出端执行"不破坏 §8 限幅约束；
// - 无状态纯函数（零分配、无虚函数、无锁），可独立单元测试。
//
// 正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
// - 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东 ✓
// - 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东 ✓
// =============================================================================
#include <cmath>
namespace drone::vtc {
/// 机体系速度（前/右/下）→ 局部 NED（北/东/地），绕下轴按航向角 ψ 旋转：
/// v_north = v_forward·cosψ − v_right·sinψ
/// v_east = v_forward·sinψ + v_right·cosψ
/// v_down = v_down（不变）
/// 参数以引用传入并就地写出旋转结果。
inline void RotateBodyToNed(double& v_forward, double& v_right, double& v_down,
double yaw_rad) {
(void)v_down; // 垂直分量不受绕下轴旋转影响；保留参数以保持接口对称
const double c = std::cos(yaw_rad);
const double s = std::sin(yaw_rad);
const double north = v_forward * c - v_right * s;
const double east = v_forward * s + v_right * c;
v_forward = north;
v_right = east;
// v_down 不变
}
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h
+31
View File
@@ -0,0 +1,31 @@
#pragma once
// =============================================================================
// i_command_transport.h —— 命令传输层抽象接口
//
// 设计目标（双重解耦）：
// - 传输层只吃字节流：串口 / 电台 / 回环打印均可注入实现，
// 后续重写传输只换实现，不动打包与控制律；
// - 打包封装（MavlinkCommandGateway）与本接口组合，不直接依赖具体链路。
//
// 控制律不直接写飞控（物理追踪思路 §2.2 控制单出口约束的独立库映射）：
// 网关打包后交由实现方决定如何送达。
// =============================================================================
#include <cstddef>
#include <cstdint>
namespace drone::vtc {
/// 命令传输接口：只吃字节流
class ICommandTransport {
public:
virtual ~ICommandTransport() = default;
/// 发送一帧已打包的命令字节流。
/// 返回 true = 已交付底层链路；false = 发送失败（调用方可计数/告警）。
/// 实现方保证：不长期持有 data 指针（同步拷贝或同步写出）。
virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h
+38
View File
@@ -0,0 +1,38 @@
#pragma once
// =============================================================================
// line_of_sight.h —— 像素偏移 → 视线角（针孔模型）
//
// 对应规格：docs/物理追踪思路.md
// §4.1 水平视线角 β：
// 针孔模型 β = atan( x_offset · 2·tan(θ_h/2) / W )
// （线性近似 β ≈ x_offset · ( θ_h / W ) 仅在 |x_offset| 很小时成立）
// §5.1 垂直视线角 α 同理（θ_v、H）
//
// 实现要点：
// - 全程使用精确针孔模型：规格中的线性近似与针孔式在切换边界两侧
// 函数值不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制
// 环路注入突变；atan 计算代价可忽略，故直接全程采用精确式，
// linear_ratio 配置项随之移除（§8 待定项收敛）；
// - 构造期预计算针孔系数，热路径仅一次乘加 + atan
// （零分配、无虚函数、无锁）。
// =============================================================================
namespace drone::vtc {
/// 像素偏移 → 视线角（弧度）转换器；水平/垂直各构造一个实例
class PixelToAngle {
public:
/// size_px：画面尺寸（W 或 H，像素，必须 > 0）；
/// fov_rad：视场角（θ_h 或 θ_v，弧度，必须 ∈ (0, π)）。
/// 非法参数抛 std::invalid_argument（构造期校验）。
PixelToAngle(double size_px, double fov_rad);
/// 偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）
double Convert(double offset_px) const;
private:
double pinhole_scale_; // 2·tan(θ/2) / size：针孔模型系数
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h
+65
View File
@@ -0,0 +1,65 @@
#pragma once
// =============================================================================
// mavlink_command_gateway.h —— MAVLink 命令打包网关（对外声明）
//
// 对应规格：docs/物理追踪思路.md §2.2
// - kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式）
// - kBrakeHover → ID 84 全零速度
// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h）
//
// 关键构建约束（务必遵守）：
// - MAVLink 头（common/mavlink.h）只允许被本库的
// src/mavlink_command_gateway.cpp 一个 TU 包含：
// MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义；
// - 因此本头文件不出现任何 mavlink_* 类型，打包/发送缓冲用
// 固定尺寸字节存储承载（尺寸在 .cpp 中 static_assert 校验）。
//
// 线程与资源：无线程、无 IO、公共方法非虚；成员固定缓冲，热路径零堆分配。
// =============================================================================
#include <cstddef>
#include <cstdint>
#include "visual_track_control/i_command_transport.h"
#include "visual_track_control/vtc_types.h"
namespace drone::vtc {
/// MAVLink 命令打包网关：控制意图 → MAVLink 消息字节流 → 传输接口
class MavlinkCommandGateway {
public:
/// transport：字节流传输实现（注入，生命周期由调用方保证）；
/// system_id/component_id：本机 MAVLink 身份；
/// target_system/target_component：飞控端身份。
MavlinkCommandGateway(ICommandTransport& transport, std::uint8_t system_id,
std::uint8_t component_id, std::uint8_t target_system,
std::uint8_t target_component);
/// 速度与航向意图 → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式 type_mask）。
/// 掩码选择：`yaw_rate_dps` 非零 → 角速率掩码；`yaw_deg` 非零 → 航向角掩码；
/// 两者均为零 → 角速率掩码配零角速率（保持当前航向，避免误发绝对航向 0）。
bool SendVelocityHeading(const ControlOutput& output, std::uint32_t time_boot_ms);
/// 零速刹车 → ID 84 全零速度（判丢等场景，§6.4）。
bool SendBrakeHover(std::uint32_t time_boot_ms);
// 固定缓冲尺寸（.cpp 中与实际类型大小做 static_assert 校验）：
static constexpr std::size_t kMsgBufferSize = 512; // ≥ sizeof(mavlink_message_t)
static constexpr std::size_t kTxBufferSize = 512; // ≥ MAVLINK_MAX_PACKET_LEN
private:
ICommandTransport& transport_; // 字节流传输实现（注入）
// MAVLink 身份
std::uint8_t system_id_;
std::uint8_t component_id_;
std::uint8_t target_system_;
std::uint8_t target_component_;
// 固定缓冲：热路径零堆分配（对齐 8 满足 mavlink_message_t 对齐要求）
alignas(8) std::uint8_t msg_storage_[kMsgBufferSize]; // mavlink_message_t 承载
std::uint8_t tx_buffer_[kTxBufferSize]; // 发送字节缓冲
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/pid_controller.h
+59
View File
@@ -0,0 +1,59 @@
#pragma once
// =============================================================================
// pid_controller.h —— 位置式 PID 控制器（纯算法构件）
//
// 对应规格：docs/物理追踪思路.md
// §6.2 距离控制律：前向速度指令 = PID(Δd)，Δd = d − d_exp
// §8 PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部待定/配置化
//
// 特性（热路径零分配、无虚函数、无锁）：
// - 位置式输出：out = Kp·e + Ki·∫e + Kd·d(e)/dt；
// - 积分钳位：积分项限幅在 ±integral_limit；
// - anti-windup（条件积分）：输出饱和且本拍积分增量仍推深饱和时冻结积分，
// 误差反号后允许积分回卷，防止积分饱和与残留；
// - 微分一阶低通：derivative_filter_coef ∈ [0,1)，0 = 不滤波；
// - ResetIntegral() / Reset() 均 O(1)（Coast 清积分，§6.4）。
// =============================================================================
namespace drone::vtc {
/// PID 参数（全部来自配置，代码不写死任何数值）
struct PidParams {
double kp = 0.0; // 比例增益
double ki = 0.0; // 积分增益
double kd = 0.0; // 微分增益
double integral_limit = 0.0; // 积分项限幅（<=0 表示不限幅）
double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：越大滤波越强
double output_limit = 0.0; // 输出限幅（<=0 表示不限幅）
};
/// 位置式 PID 控制器
class PidController {
public:
explicit PidController(const PidParams& params);
/// 单步更新：输入误差与采样间隔（秒），返回限幅后的输出。
/// dt_s <= 0 时保持上次输出（异常节拍保护）。
double Update(double error, double dt_s);
/// 清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出
void ResetIntegral();
/// 清空全部内部状态（积分、微分历史、上次输出）
void Reset();
/// 上次输出（只读）
double LastOutput() const { return last_output_; }
private:
PidParams params_;
double integral_ = 0.0; // 积分累计（已钳位）
double prev_error_ = 0.0; // 上次误差（微分用）
double filtered_derivative_ = 0.0; // 低通后的微分值
double last_output_ = 0.0; // 上次输出
bool has_prev_error_ = false; // 是否存在有效微分历史
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h
+56
View File
@@ -0,0 +1,56 @@
#pragma once
// =============================================================================
// rate_limiter.h —— 限幅与变化率限制（纯算法构件，header-only）
//
// 对应规格：docs/物理追踪思路.md
// §8 所有阈值集中配置：速度/加速度/方向变化率必须限幅（FR-033）
// S6 首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
// （状态机设计.md S6）
//
// 特性（热路径零分配、无虚函数、无锁）：
// - Clamp()：静态限幅；
// - SlewRateLimiter：保存上次输出，按配置变化率限幅；初值置 0 时
// 天然实现"首条指令突变抑制"（S6）。
// =============================================================================
#include <algorithm>
namespace drone::vtc {
/// 限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）
inline double Clamp(double v, double lo, double hi) {
return std::min(std::max(v, lo), hi);
}
/// 变化率限幅器：限制输出每秒最大变化量（单位由调用方决定，
/// 如 度/秒^2 用于航向、米/秒^2 用于速度）
class SlewRateLimiter {
public:
/// rate_limit：每秒最大变化量（<=0 表示冻结输出）；initial：初始输出值
explicit SlewRateLimiter(double rate_limit, double initial = 0.0)
: rate_limit_(rate_limit), output_(initial) {}
/// 朝 target 推进一个节拍：单步变化不超过 rate_limit·dt_s。
/// dt_s <= 0 或 rate_limit <= 0 时保持当前输出（异常节拍保护）。
double Limit(double target, double dt_s) {
if (dt_s <= 0.0 || rate_limit_ <= 0.0) {
return output_;
}
const double max_step = rate_limit_ * dt_s;
output_ = Clamp(target, output_ - max_step, output_ + max_step);
return output_;
}
/// 复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）
void Reset(double value = 0.0) { output_ = value; }
/// 上次输出（只读）
double LastOutput() const { return output_; }
private:
double rate_limit_; // 每秒最大变化量
double output_; // 当前输出
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h
+38
View File
@@ -0,0 +1,38 @@
#pragma once
// =============================================================================
// staleness_checker.h —— 输入超龄判定（单调毫秒时钟）
//
// 对应规格：docs/物理追踪思路.md
// §7.1 超龄数据只能触发降级或告警，不能继续生成正常控制指令；
// 超龄判定使用取样时间 + valid_for_ms（MessageHeader 语义）。
//
// 特性：无状态纯函数封装（零分配、无虚函数、无锁）。
// =============================================================================
#include <cstdint>
namespace drone::vtc {
/// 输入超龄判定器（无状态，仅提供静态判定）
class StalenessChecker {
public:
/// sample_time_ms：取样单调时钟毫秒（<=0 视为从未取样，直接判超龄）；
/// now_ms：当前单调时钟毫秒；
/// valid_for_ms：有效期（毫秒，配置给定）。
/// 返回 true = 已超龄。
static bool IsStale(std::int64_t sample_time_ms, std::int64_t now_ms,
std::int64_t valid_for_ms) {
if (sample_time_ms <= 0) {
return true; // 从未取样：视为超龄（触发降级）
}
if (now_ms < sample_time_ms) {
// 时间戳不一致（时钟源错配/打戳错误）：年龄不可信。
// 单一单调时钟下不应发生；宁可降级也不信任不可核实的取样。
return true;
}
return (now_ms - sample_time_ms) > valid_for_ms;
}
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h
+63
View File
@@ -0,0 +1,63 @@
#pragma once
// =============================================================================
// tracking_control_law.h —— 视觉跟踪主控律（TrackingControlLaw）
//
// 对应规格：docs/物理追踪思路.md
// §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置）
// §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并）
// §6 距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3）
// §6.4 Coast 清积分、判丢零速刹车
// §7.1 超龄判定（姿态/视觉超龄 → 降级保持）
// §8 统一限幅
// §2.3 输出端随机体航向把机体系速度旋转为局部 NED
//
// 线程模型：单线程调用（状态机与控制线程），成员无锁、无堆对象；
// 日志仅状态切换点（刹车进出、降级切换），高频热路径不打日志。
// =============================================================================
#include <cstdint>
#include "visual_track_control/line_of_sight.h"
#include "visual_track_control/pid_controller.h"
#include "visual_track_control/rate_limiter.h"
#include "visual_track_control/vtc_config.h"
#include "visual_track_control/vtc_types.h"
namespace drone::vtc {
/// 视觉跟踪主控律：消费控制快照，输出限幅后的控制意图
class TrackingControlLaw {
public:
/// cfg：启动期加载的全量配置（值拷贝持有，运行期不再触碰配置源）
explicit TrackingControlLaw(const TrackingConfig& cfg);
/// 单周期控制计算（§7.1 固定周期持续运行）。
/// now_ms：当前单调时钟毫秒；内部据此推算 dt 并做超龄判定。
/// 流程：①超龄判定 → ②判丢刹车 → ③Coast 清积分 → ④像素→视线角
/// → ⑤水平航向 → ⑥垂直速度 → ⑦距离通道 → ⑧机体系→NED 旋转输出
ControlOutput Update(const ControlSnapshot& snapshot, std::int64_t now_ms);
/// 停止视觉闭环时清全部状态（积分、限幅器历史、节拍时钟，§6.4）
void Reset();
private:
TrackingConfig cfg_; // 配置值拷贝（运行期只读）
// ---- 预计算构件（构造期完成，热路径零分配） ----
PixelToAngle pixel_to_beta_; // 水平：x_offset → β（§4.1）
PixelToAngle pixel_to_alpha_; // 垂直：y_offset → α（§5.1）
PidController distance_pid_; // 距离通道 PID（§6.2）
// ---- 变化率限幅器（§8 / S6 首条指令限幅） ----
SlewRateLimiter yaw_deg_limiter_; // 位置式航向变化率（度/秒^2）
SlewRateLimiter yaw_rate_limiter_; // 速率式角速度变化率（度/秒^2）
SlewRateLimiter vx_limiter_; // 前向加速度限幅（米/秒^2）
SlewRateLimiter vz_limiter_; // 垂直加速度限幅（米/秒^2）
// ---- 节拍与状态（仅打日志用，不参与数学） ----
std::int64_t last_now_ms_ = 0; // 上次计算时刻
ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志）
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/vtc_config.h
+113
View File
@@ -0,0 +1,113 @@
#pragma once
// =============================================================================
// vtc_config.h —— 视觉跟踪控制律配置（TrackingConfig + LoadFromJson）
//
// 对应规格：docs/物理追踪思路.md
// §8 限幅、阈值与配置化（所有阈值集中配置，不得散落在代码中，
// 需求分析.md NFR-007）
// §4 水平航向控制（模式、增益、限幅）
// §5 垂直/高度控制（垂直速度增益与限幅；原双支路已统一为速度支路）
// §6 距离控制（射程、PID、限幅、无距离降级动作）
// §7 控制周期与时序（频率、各输入超龄阈值）
//
// 设计约束：
// - 全部数值待定：本结构默认值与示例配置均为占位，标定前仅限回放验证；
// - LoadFromJson 启动期一次性解析：缺字段/类型错/非法值抛
// std::runtime_error 并携带字段名（初始化抛异常规范）；
// - 热路径绝不触碰 JSON 对象，不做热更新（调参重启即可）。
// =============================================================================
#include <cstdint>
#include <string>
namespace drone::vtc {
/// 航向通道模式（§4.2：位置式/速率式按 PX4 1.11.3 Offboard 实测选择）
///
/// 默认速率式：yaw_rate 指令天然是相对量，无坐标系歧义。位置式输出为
/// 绝对 NED 航向（当前航向 + 限幅修正量合成），依赖新鲜自机姿态；
/// 姿态接线未完成前不得用于实飞。
enum class HeadingMode : std::uint8_t {
kRate, // 速率式（默认）：β → 偏航角速度，输出 yaw_rate_dps
kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg
};
/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定）
enum class NoDistanceAction : std::uint8_t {
kHold, // 保持：前向速度指令清零（安全占位语义）
kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近
kExit, // 退出近距操作：输出降级保持（kDegradedHold）
};
/// control 组：控制周期与各输入超龄阈值（§7）
struct ControlGroupConfig {
double frequency_hz = 20.0; // 控制频率（Hz，§7.1 待定）
std::int64_t visual_stale_ms = 200; // 视觉追踪结果超龄阈值（毫秒）
std::int64_t radar_stale_ms = 500; // 雷达距离超龄阈值（毫秒）
std::int64_t attitude_stale_ms = 300; // 自机姿态超龄阈值（毫秒）
};
/// camera 组：画面与视场角（§3.2、§4.1、§5.1；内参/畸变未标定，占位）
struct CameraGroupConfig {
int image_width = 1920; // 画面宽度 W（像素）
int image_height = 1080; // 画面高度 H（像素）
double fov_h_deg = 60.0; // 水平视场角 θ_h（度，仅水平，§4.1）
double fov_v_deg = 35.0; // 垂直视场角 θ_v（度，§5.1）
};
/// heading 组：水平航向通道（§4）
struct HeadingGroupConfig {
HeadingMode mode = HeadingMode::kRate; // 默认速率式（§4.2，见 HeadingMode 注释）
double gain = 1.0; // 增益：视线角(度) → 航向指令（无量纲占位）
double yaw_rate_limit_dps = 30.0; // 偏航角速度上限（度/秒，FR-033）
double yaw_slew_limit_dps2 = 60.0; // 方向变化率上限（度/秒^2，FR-033、S6 首条限幅）
};
/// vertical 组：垂直/高度控制（§5）
///
/// 规格 §5.2 原设"小误差姿态微调 / 大误差垂直速度"双支路，实测评审后
/// 统一为速度支路（见 ControlMode 注释）：切换阈值与姿态微调参数随之移除。
/// 小误差经 vz_gain 线性映射为小垂直速度，行为连续且无模式翻转。
struct VerticalGroupConfig {
double vz_gain_mps_per_deg = 0.1; // 垂直速度增益：α(度) → 垂直速度(米/秒)
double vz_limit_mps = 2.0; // 垂直速度限幅（米/秒）
};
/// distance 组：距离通道（§6）
struct DistanceGroupConfig {
double d_exp_m = 10.0; // 射程 d_exp（米，由拦截设备接口给出，§6.1 待定）
double kp = 0.0; // PID 比例（§6.2 待定）
double ki = 0.0; // PID 积分
double kd = 0.0; // PID 微分
double integral_limit = 0.0; // 积分项限幅
double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：0=不滤波
double approach_velocity_limit_mps = 3.0; // 接近速度限幅（米/秒，§8）
double retreat_velocity_limit_mps = 1.0; // 后退速度限幅（米/秒，§8）
NoDistanceAction no_distance_action = NoDistanceAction::kHold; // 无距离降级（§6.3）
double no_distance_approach_limit_mps = 0.5; // kSlowApproach 限速值（米/秒）
};
/// accel_limit 组：各轴加速度限幅（§8，待定）
struct AccelLimitConfig {
double ax_mps2 = 2.0; // 前向加速度限幅（米/秒^2）
double ay_mps2 = 2.0; // 侧向加速度限幅（米/秒^2，预留）
double az_mps2 = 1.5; // 垂直加速度限幅（米/秒^2）
};
/// 视觉跟踪控制律全量配置（§8：所有阈值集中于此）
struct TrackingConfig {
ControlGroupConfig control;
CameraGroupConfig camera;
HeadingGroupConfig heading;
VerticalGroupConfig vertical;
DistanceGroupConfig distance;
AccelLimitConfig accel_limit;
/// 从 JSON 文件一次性加载配置（启动期调用）。
/// 缺字段 / 类型错误 / 非法取值均抛 std::runtime_error，
/// 异常消息携带字段名（如 "heading.gain"）。
static TrackingConfig LoadFromJson(const std::string& path);
};
} // namespace drone::vtc
reference-c++/visual_track_control/include/visual_track_control/vtc_types.h
+102
View File
@@ -0,0 +1,102 @@
#pragma once
// =============================================================================
// vtc_types.h —— 视觉跟踪控制律的中立输入/输出结构
//
// 对应规格：docs/物理追踪思路.md
// §2 输入输出与数据流（控制意图枚举语义）
// §6.4 与追踪库的行为衔接（tracked / is_predicted 语义）
// §7.2 与 target_tracker 的衔接（TrackResult 无时间戳，由调用方在
// LastResult() 瞬间打戳）
//
// 设计约束：
// - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格），
// 控制律热路径零堆分配；
// - 本库不依赖主工程 include/ 的任何类型，独立可编译；
// - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作
// 目标距离（§6.3、需求分析.md FR-041）。
// =============================================================================
#include <cstdint>
#include <target_tracker/target_tracker.h>
namespace drone::vtc {
/// 控制输出模式（对应物理追踪思路 §2.2 的控制意图语义）
///
/// 注意：规格 §5.2 原设"垂直小误差姿态微调支路"（kAttitudeTrim →
/// SET_ATTITUDE_TARGET），实测评审后取消——姿态微调支路会把整个输出切成
/// 姿态消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息
/// 类型逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路（小误差
/// 对应小 vz），全周期单一 offboard 消息。
enum class ControlMode : std::uint8_t {
kVelocityHeading, // 速度与航向意图：速度分量 + yaw（§4.2、§5、§6.2 主输出）
kBrakeHover, // 零速刹车/悬停：判丢等场景（§6.4、§9.1）
kDegradedHold, // 降级保持：超龄/退出近距操作，禁止正常控制（§7.1、§9.2）
};
/// 降级原因（仅 kDegradedHold 模式有意义）
enum class DegradedReason : std::uint8_t {
kNone, // 未降级
kAttitudeStale, // PX4 姿态过期：禁止空间转换和控制（§9.2）
kVisionStale, // 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
kNoDistanceExit, // 无距离关联且配置为退出近距操作（§6.3）
};
/// 控制律输入快照：调用方在同一控制周期内采集并打戳
///
/// 注意：TrackResult 本身不含时间戳（§7.2），调用方必须在读取
/// LastResult() 的瞬间用单调时钟打戳填入 track_time_ms；
/// 超龄判定一律使用"取样时间 + valid_for_ms"（§7.1）。
struct ControlSnapshot {
// ---- 视觉追踪结果（控制律主输入） ----
drone::tracker::TrackResult track; // 追踪库结果（值拷贝）
std::int64_t track_time_ms = 0; // 取样单调时钟（毫秒），<=0 视为从未取样
// ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ----
double radar_distance_m = 0.0; // 机头前向单点距离（米）
bool associated_to_target = false; // 相机—雷达空间关联成立标志
std::int64_t radar_time_ms = 0; // 取样单调时钟（毫秒）
// ---- 自机姿态/速度 ----
// 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转
// （§2.3）与位置式绝对航向合成；其余字段预留，接入主工程时由
// FlightStateSnapshot 填充（机体补偿见 §4.1）。
bool attitude_present = false; // 姿态源当前是否可用
std::int64_t attitude_time_ms = 0; // 姿态取样单调时钟（毫秒）
double roll_rad = 0.0; // 横滚（弧度，FRD）
double pitch_rad = 0.0; // 俯仰（弧度，FRD）
double yaw_rad = 0.0; // 航向（弧度，NED）
double yaw_rate_radps = 0.0; // 偏航角速度（弧度/秒，机体补偿预留，§4.1）
double velocity_x_mps = 0.0; // 自机速度分量（预留）
double velocity_y_mps = 0.0;
double velocity_z_mps = 0.0;
};
/// 控制律输出：限幅后的控制意图（§8：所有输出必须经过限幅）
///
/// 坐标语义：速度分量在机体系（前/右/下，§2.3）内计算与限幅，输出前
/// 已随自机航向（ControlSnapshot.yaw_rad）旋转为局部 NED（北/东/地），
/// 与网关使用的 MAV_FRAME_LOCAL_NED 一致；因此旋转后 vy_mps 可能非零
/// （机体系侧向恒为 0，旋转不改变合速度大小，限幅在旋转前完成）。
struct ControlOutput {
ControlMode mode = ControlMode::kDegradedHold; // 输出模式
bool valid = false; // 指令是否可用于发送
// ---- 速度分量（局部 NED：北/东/地，米/秒；由机体系随航向旋转得到） ----
double vx_mps = 0.0; // 北向（距离通道输出经旋转，§6.2）
double vy_mps = 0.0; // 东向（机体系恒 0，航向旋转后可非零）
double vz_mps = 0.0; // 垂直（NED 下正，垂直速度支路输出，§5.2）
// ---- 航向通道（§4.2：位置式输出 yaw_deg，速率式输出 yaw_rate_dps） ----
double yaw_deg = 0.0; // 目标绝对航向（度，NED，右转为正；仅位置式，
// 由当前航向 + 限幅修正量合成并回绕 [-180,180)）
double yaw_rate_dps = 0.0; // 偏航角速度指令（度/秒；速率式为默认，天然相对量）
// ---- 状态标注 ----
DegradedReason degraded_reason = DegradedReason::kNone; // 降级原因
bool coast_active = false; // 本周期处于 Coast（is_predicted==true，§6.4）
};
} // namespace drone::vtc
reference-c++/visual_track_control/src/line_of_sight.cpp
+42
View File
@@ -0,0 +1,42 @@
// =============================================================================
// line_of_sight.cpp —— 像素偏移 → 视线角实现（全程针孔模型）
//
// 对应规格：docs/物理追踪思路.md §4.1（水平 β）、§5.1（垂直 α）：
// 针孔模型：angle = atan( offset · 2·tan(θ/2) / size )
//
// 原设计的线性近似双路切换已取消：线性式与针孔式在切换边界两侧不连续
// （θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入突变；
// atan 代价可忽略，全程采用精确针孔式。
//
// 构造期预计算针孔系数，Convert 热路径仅一次乘加 + atan。
// =============================================================================
#include "visual_track_control/line_of_sight.h"
#include <cmath>
#include <stdexcept>
namespace drone::vtc {
namespace {
constexpr double kPi = 3.14159265358979323846;
}
PixelToAngle::PixelToAngle(double size_px, double fov_rad) {
// 构造期校验：非法参数直接抛异常（初始化抛异常规范）
if (!(size_px > 0.0)) {
throw std::invalid_argument("PixelToAngle: size_px 必须为正");
}
if (!(fov_rad > 0.0 && fov_rad < kPi)) {
throw std::invalid_argument("PixelToAngle: fov_rad 必须在 (0, π) 内");
}
// 预计算针孔系数（§4.1 公式）
pinhole_scale_ = 2.0 * std::tan(fov_rad / 2.0) / size_px; // 2·tan(θ/2) / size
}
double PixelToAngle::Convert(double offset_px) const {
return std::atan(offset_px * pinhole_scale_); // 针孔模型，全偏移范围精确
}
} // namespace drone::vtc
reference-c++/visual_track_control/src/mavlink_command_gateway.cpp
+119
View File
@@ -0,0 +1,119 @@
// =============================================================================
// mavlink_command_gateway.cpp —— MAVLink 命令打包网关实现
//
// 对应规格：docs/物理追踪思路.md §2.2
// kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式）
// kBrakeHover → ID 84 全零速度
// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h）
//
// 关键构建约束：本文件是全工程唯一包含 common/mavlink.h 的 TU。
// MAVLINK_HELPER 默认空宏（mavlink_helpers.h），多 TU 包含会多重定义。
//
// 打包策略：
// - 使用 mavlink_msg_*_pack（无通道状态、纯静态内联、可重入），
// 不使用 _pack_chan 通道 API；
// - mavlink_msg_to_send_buffer 输出字节流 → ICommandTransport::Send；
// - 消息/发送缓冲为成员固定持有，热路径零堆分配。
//
// 坐标系与语义说明（待实测确认）：
// - coordinate_frame 取 MAV_FRAME_LOCAL_NED；速度分量由主控律在输出端
// 随机体航向旋转为局部 NED（北/东/地，物理追踪思路 §2.3），本文件
// 直接填充不再做坐标变换；
// - 速度模式 type_mask 需以 PX4 1.11.3 Offboard 实测为准
// （物理追踪思路 §10.1）。
// =============================================================================
#include "visual_track_control/mavlink_command_gateway.h"
#include <cmath>
#include "common/mavlink.h" // 全工程唯一包含点（见文件头构建约束）
namespace drone::vtc {
namespace {
constexpr double kPi = 3.14159265358979323846;
constexpr double kDegToRad = kPi / 180.0;
// ---- SET_POSITION_TARGET_LOCAL_NED（ID 84）type_mask 常量 ----
// 掩码位语义：置位 = 忽略该字段。
// 基础位：忽略位置 x/y/z（bit0-2）与加速度 afx/afy/afz（bit6-8）= 0x01C7。
constexpr std::uint16_t kMaskVelocityBase = 0x01C7;
// 速度 + 航向角模式：忽略 yaw_rate（bit11）
constexpr std::uint16_t kMaskVelocityYaw = kMaskVelocityBase | 0x0800;
// 速度 + 偏航角速率模式：忽略 yaw（bit10）
constexpr std::uint16_t kMaskVelocityYawRate = kMaskVelocityBase | 0x0400;
// 刹车模式：速度全零，yaw 与 yaw_rate 均忽略（bit10、bit11）
constexpr std::uint16_t kMaskBrake = kMaskVelocityBase | 0x0C00;
/// 固定存储解释为消息结构（打包函数只写入字节，不读取旧值）
inline mavlink_message_t* AsMessage(std::uint8_t* storage) {
return reinterpret_cast<mavlink_message_t*>(storage);
}
} // namespace
// 固定缓冲尺寸校验：编译期保证存储足够（头文件不暴露 mavlink 类型）
static_assert(sizeof(mavlink_message_t) <= MavlinkCommandGateway::kMsgBufferSize,
"msg_storage_ 不足以容纳 mavlink_message_t");
static_assert(MAVLINK_MAX_PACKET_LEN <= MavlinkCommandGateway::kTxBufferSize,
"tx_buffer_ 不足以容纳最大 MAVLink 数据包");
MavlinkCommandGateway::MavlinkCommandGateway(ICommandTransport& transport,
std::uint8_t system_id,
std::uint8_t component_id,
std::uint8_t target_system,
std::uint8_t target_component)
: transport_(transport),
system_id_(system_id),
component_id_(component_id),
target_system_(target_system),
target_component_(target_component) {}
bool MavlinkCommandGateway::SendVelocityHeading(const ControlOutput& output,
std::uint32_t time_boot_ms) {
auto* msg = AsMessage(msg_storage_);
// 掩码三分支（A2 修复）：
// 角速率非零 → 速率掩码（速率式控制律常态，§4.2）；
// 航向角非零 → 航向掩码（位置式控制律）；
// 两者均零 → 速率掩码配零角速率：保持当前航向。
// 旧实现把"零角速率"落到航向掩码，等于误发绝对航向 0（指向正北）。
const bool has_yaw_rate = std::fabs(output.yaw_rate_dps) > 1.0e-9;
const bool has_yaw = std::fabs(output.yaw_deg) > 1.0e-9;
const std::uint16_t type_mask =
(!has_yaw_rate && has_yaw) ? kMaskVelocityYaw : kMaskVelocityYawRate;
mavlink_msg_set_position_target_local_ned_pack(
system_id_, component_id_, msg,
time_boot_ms, target_system_, target_component_,
MAV_FRAME_LOCAL_NED, type_mask,
0.0F, 0.0F, 0.0F, // 位置字段：忽略
static_cast<float>(output.vx_mps), static_cast<float>(output.vy_mps),
static_cast<float>(output.vz_mps),
0.0F, 0.0F, 0.0F, // 加速度字段：忽略
static_cast<float>(output.yaw_deg * kDegToRad),
static_cast<float>(output.yaw_rate_dps * kDegToRad));
const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg);
return transport_.Send(tx_buffer_, len);
}
bool MavlinkCommandGateway::SendBrakeHover(std::uint32_t time_boot_ms) {
auto* msg = AsMessage(msg_storage_);
// 判丢零速刹车（§6.4）：速度全零，位置/加速度/航向字段全部忽略
mavlink_msg_set_position_target_local_ned_pack(
system_id_, component_id_, msg,
time_boot_ms, target_system_, target_component_,
MAV_FRAME_LOCAL_NED, kMaskBrake,
0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F,
0.0F, 0.0F);
const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg);
return transport_.Send(tx_buffer_, len);
}
} // namespace drone::vtc
reference-c++/visual_track_control/src/pid_controller.cpp
+81
View File
@@ -0,0 +1,81 @@
// =============================================================================
// pid_controller.cpp —— 位置式 PID 控制器实现
//
// 对应规格：docs/物理追踪思路.md
// §6.2 前向速度指令 = PID(Δd)
// §6.4 Coast 时积分项清空（ResetIntegral）
//
// anti-windup 策略（条件积分）：输出达到饱和限幅且本拍积分增量仍在
// 推深饱和时，冻结本步积分累加（不采纳候选积分值），防止积分饱和；
// 误差反号后增量反向、允许积分回卷，避免饱和残留。
// =============================================================================
#include "visual_track_control/pid_controller.h"
#include <cmath>
#include "visual_track_control/rate_limiter.h"
namespace drone::vtc {
PidController::PidController(const PidParams& params) : params_(params) {}
double PidController::Update(double error, double dt_s) {
if (dt_s <= 0.0) {
return last_output_; // 异常节拍保护：保持上次输出
}
// ---- 积分：候选值先钳位（积分限幅） ----
double integral_candidate = integral_ + error * dt_s;
if (params_.integral_limit > 0.0) {
integral_candidate =
Clamp(integral_candidate, -params_.integral_limit, params_.integral_limit);
}
// ---- 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） ----
double derivative = 0.0;
if (has_prev_error_) {
derivative = (error - prev_error_) / dt_s;
const double coef = params_.derivative_filter_coef;
derivative = coef * filtered_derivative_ + (1.0 - coef) * derivative;
}
filtered_derivative_ = derivative;
prev_error_ = error;
has_prev_error_ = true;
// ---- 位置式合成 ----
const double integral_term = params_.ki * integral_candidate;
const double raw = params_.kp * error + integral_term + params_.kd * derivative;
// ---- anti-windup（条件积分，C1 修复） ----
// 冻结判据基于"本拍积分增量方向"（error·dt）：输出饱和且本拍增量仍
// 在推深饱和时才冻结。不采用旧实现"积分项与输出同向"判据（误差反号
// 后无法回卷），也不用 raw·error（大 kp 项会掩盖饱和方向、拖延回卷）。
const bool saturated = params_.output_limit > 0.0 && std::fabs(raw) > params_.output_limit;
const bool increment_pushes_deeper = (error * dt_s) * raw > 0.0;
if (!(saturated && increment_pushes_deeper)) {
integral_ = integral_candidate;
}
// ---- 输出限幅 ----
if (params_.output_limit > 0.0) {
last_output_ = Clamp(raw, -params_.output_limit, params_.output_limit);
} else {
last_output_ = raw;
}
return last_output_;
}
void PidController::ResetIntegral() {
integral_ = 0.0; // §6.4：Coast 清积分，防积分饱和
}
void PidController::Reset() {
integral_ = 0.0;
prev_error_ = 0.0;
filtered_derivative_ = 0.0;
last_output_ = 0.0;
has_prev_error_ = false;
}
} // namespace drone::vtc
reference-c++/visual_track_control/src/tracking_control_law.cpp
+239
View File
@@ -0,0 +1,239 @@
// =============================================================================
// tracking_control_law.cpp —— 视觉跟踪主控律实现
//
// 对应规格：docs/物理追踪思路.md
// §4.1/§4.2 水平航向控制：像素偏移 → β → 增益 → slew → 角速度限幅，
// 位置式输出 yaw_deg / 速率式输出 yaw_rate_dps（按配置）
// §5.1/§5.2 垂直控制：统一速度支路 α → 垂直速度设定值（原"小误差姿态
// 微调/大误差垂直速度"双支路已合并：姿态微调会把输出切成姿态
// 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见
// vtc_types.h ControlMode 注释）
// §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）；
// 未关联/超龄 → 按配置降级动作（hold / slow_approach / exit）
// §6.4 Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车
// §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持
// §8 所有输出统一限幅；首条指令经变化率限幅（S6）
// §2.3 输出端随机体航向把机体系速度旋转为局部 NED
//
// 日志纪律：仅状态切换点（刹车进出、降级切换）打印；热路径零日志、零分配。
// =============================================================================
#include "visual_track_control/tracking_control_law.h"
#include <algorithm>
#include <cmath>
#include <spdlog/spdlog.h>
#include "visual_track_control/frame_transform.h"
#include "visual_track_control/staleness_checker.h"
namespace drone::vtc {
namespace {
constexpr double kPi = 3.14159265358979323846;
constexpr double kRadToDeg = 180.0 / kPi;
constexpr double kDegToRad = kPi / 180.0;
constexpr double kMaxDtS = 0.5; // 单步 dt 上限（秒）：异常节拍保护
/// 角度回绕到 [-180, 180)（位置式绝对航向合成用）
double WrapDeg180(double deg) {
double d = std::fmod(deg + 180.0, 360.0);
if (d < 0.0) {
d += 360.0;
}
return d - 180.0;
}
/// 控制模式可读名（仅状态切换日志使用）
const char* ModeName(ControlMode mode) {
switch (mode) {
case ControlMode::kVelocityHeading: return "速度航向";
case ControlMode::kBrakeHover: return "零速刹车";
case ControlMode::kDegradedHold: return "降级保持";
}
return "未知";
}
/// 由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者
PidParams BuildDistancePidParams(const TrackingConfig& cfg) {
PidParams p;
p.kp = cfg.distance.kp;
p.ki = cfg.distance.ki;
p.kd = cfg.distance.kd;
p.integral_limit = cfg.distance.integral_limit;
p.derivative_filter_coef = cfg.distance.derivative_filter_coef;
p.output_limit = std::max(cfg.distance.approach_velocity_limit_mps,
cfg.distance.retreat_velocity_limit_mps);
return p;
}
} // namespace
TrackingControlLaw::TrackingControlLaw(const TrackingConfig& cfg)
: cfg_(cfg),
pixel_to_beta_(cfg.camera.image_width, cfg.camera.fov_h_deg * kDegToRad),
pixel_to_alpha_(cfg.camera.image_height, cfg.camera.fov_v_deg * kDegToRad),
distance_pid_(BuildDistancePidParams(cfg)),
yaw_deg_limiter_(cfg.heading.yaw_slew_limit_dps2),
yaw_rate_limiter_(cfg.heading.yaw_slew_limit_dps2),
vx_limiter_(cfg.accel_limit.ax_mps2),
vz_limiter_(cfg.accel_limit.az_mps2) {}
ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
std::int64_t now_ms) {
// ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
// 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率：
// 单拍误差由限幅器与超龄判定兜底，不引入额外状态。
double dt_s = 1.0 / cfg_.control.frequency_hz;
if (last_now_ms_ > 0 && now_ms > last_now_ms_) {
dt_s = std::min(static_cast<double>(now_ms - last_now_ms_) / 1000.0, kMaxDtS);
}
last_now_ms_ = now_ms;
ControlOutput out;
out.mode = ControlMode::kVelocityHeading; // 正常路径默认模式，分支可覆盖
// 状态切换日志辅助：仅模式变化时打印（刹车进出、降级切换等）
auto FinishAndLog = [&](const char* reason) {
if (out.mode != last_mode_) {
spdlog::info("[ControlLaw] 状态切换: {} -> {} ({})",
ModeName(last_mode_), ModeName(out.mode), reason);
last_mode_ = out.mode;
}
return out;
};
// ---- ① 超龄判定（§7.1 / §9.2） ----
// 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2）
if (!snapshot.attitude_present ||
StalenessChecker::IsStale(snapshot.attitude_time_ms, now_ms,
cfg_.control.attitude_stale_ms)) {
out.mode = ControlMode::kDegradedHold;
out.degraded_reason = DegradedReason::kAttitudeStale;
return FinishAndLog("姿态过期/缺失，禁止控制");
}
// 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
if (StalenessChecker::IsStale(snapshot.track_time_ms, now_ms,
cfg_.control.visual_stale_ms)) {
out.mode = ControlMode::kDegradedHold;
out.degraded_reason = DegradedReason::kVisionStale;
return FinishAndLog("视觉追踪结果超龄");
}
// ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ----
if (!snapshot.track.tracked) {
if (last_mode_ != ControlMode::kBrakeHover) {
// 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
distance_pid_.Reset();
yaw_deg_limiter_.Reset();
yaw_rate_limiter_.Reset();
vx_limiter_.Reset();
vz_limiter_.Reset();
}
out.mode = ControlMode::kBrakeHover; // 速度分量保持默认全零
out.valid = true;
return FinishAndLog("判丢，零速刹车");
}
// ---- ③ Coast：is_predicted == true → 清积分防饱和（§6.4） ----
if (snapshot.track.is_predicted) {
distance_pid_.ResetIntegral();
// 航向通道为纯比例增益映射（§4.2），无积分项，无需清理
out.coast_active = true;
}
// ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ----
// x_offset = center.first − W/2（右正），y_offset = center.second − H/2（下正）
const double x_offset =
snapshot.track.center.first - cfg_.camera.image_width * 0.5;
const double y_offset =
snapshot.track.center.second - cfg_.camera.image_height * 0.5;
const double beta_deg = pixel_to_beta_.Convert(x_offset) * kRadToDeg;
const double alpha_deg = pixel_to_alpha_.Convert(y_offset) * kRadToDeg;
// ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ----
if (cfg_.heading.mode == HeadingMode::kRate) {
// 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033）
const double target_rate = cfg_.heading.gain * beta_deg;
const double slewed = yaw_rate_limiter_.Limit(target_rate, dt_s);
out.yaw_rate_dps = Clamp(slewed, -cfg_.heading.yaw_rate_limit_dps,
cfg_.heading.yaw_rate_limit_dps);
} else {
// 位置式：β → 航向修正量，变化率限幅（方向变化率上限，FR-033）后
// 与当前航向合成为绝对 NED 航向——ID 84 的 yaw 字段是绝对值，
// 直接发修正量会让机体转向"修正角"方向（A2 修复）。姿态已在 ①
// 通过超龄校验，yaw_rad 可用。
const double target_yaw = cfg_.heading.gain * beta_deg;
const double correction_deg = yaw_deg_limiter_.Limit(target_yaw, dt_s);
out.yaw_deg = WrapDeg180(snapshot.yaw_rad * kRadToDeg + correction_deg);
}
// ---- ⑥ 垂直速度通道（§5：统一速度支路） ----
// 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。
// 小误差自然映射为小垂直速度：无需单独的切换阈值与姿态微调支路，
// 输出模式保持 kVelocityHeading，距离通道前向速度不会被丢弃（A1 修复）。
{
const double vz_target = cfg_.vertical.vz_gain_mps_per_deg * alpha_deg;
const double slewed = vz_limiter_.Limit(vz_target, dt_s); // 加速度限幅（§8）
out.vz_mps = Clamp(slewed, -cfg_.vertical.vz_limit_mps,
cfg_.vertical.vz_limit_mps);
}
// ---- ⑦ 距离通道（§6.2 / §6.3） ----
// 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
const bool distance_available =
snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 &&
!StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms,
cfg_.control.radar_stale_ms);
if (distance_available) {
// Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
const double delta_d = snapshot.radar_distance_m - cfg_.distance.d_exp_m;
const double vx_raw = distance_pid_.Update(delta_d, dt_s);
const double slewed = vx_limiter_.Limit(vx_raw, dt_s); // 加速度限幅（§8）
out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps,
cfg_.distance.approach_velocity_limit_mps);
} else {
// 无可用目标距离：清积分防饱和，按配置降级动作（§6.3）
distance_pid_.ResetIntegral();
switch (cfg_.distance.no_distance_action) {
case NoDistanceAction::kHold:
// 保持：前向指令清零（安全占位语义；接入主工程后可改为保持当前速度）
out.vx_mps = 0.0;
vx_limiter_.Reset(0.0);
break;
case NoDistanceAction::kSlowApproach:
// 限速接近：缓速前向，同样经加速度限幅
out.vx_mps = vx_limiter_.Limit(
cfg_.distance.no_distance_approach_limit_mps, dt_s);
break;
case NoDistanceAction::kExit:
// 退出近距操作：整体降级保持
out.mode = ControlMode::kDegradedHold;
out.degraded_reason = DegradedReason::kNoDistanceExit;
return FinishAndLog("距离未关联，退出近距操作");
}
}
// ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
// 到此为止各通道限幅均在机体系完成（旋转为正交变换，合速度不变，
// 限幅约束不被破坏）；输出端随快照航向旋转为北/东/地，与网关的
// MAV_FRAME_LOCAL_NED 一致。姿态已在 ① 通过超龄校验，yaw_rad 可用。
RotateBodyToNed(out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad);
out.valid = true;
return FinishAndLog(out.coast_active ? "Coast 预测跟踪" : "正常跟踪");
}
void TrackingControlLaw::Reset() {
// 停止视觉闭环时清全部状态（§6.4）
distance_pid_.Reset();
yaw_deg_limiter_.Reset();
yaw_rate_limiter_.Reset();
vx_limiter_.Reset();
vz_limiter_.Reset();
last_now_ms_ = 0;
last_mode_ = ControlMode::kDegradedHold;
}
} // namespace drone::vtc
reference-c++/visual_track_control/src/vtc_config.cpp
+225
View File
@@ -0,0 +1,225 @@
// =============================================================================
// vtc_config.cpp —— 配置加载器实现（启动期一次性解析）
//
// 对应规格：docs/物理追踪思路.md §8（阈值集中配置化，NFR-007）。
//
// 实现要点：
// - 仅在启动期调用一次：缺字段 / 类型错误 / 非法取值抛
// std::runtime_error，异常消息携带完整字段名（如 "heading.gain"）；
// - 热路径绝不触碰 JSON：解析结果拷贝进 TrackingConfig 后即释放；
// - 本文件是全库唯一包含 nlohmann/json 的 TU。
// =============================================================================
#include "visual_track_control/vtc_config.h"
#include <fstream>
#include <stdexcept>
#include <string>
#include <nlohmann/json.hpp>
namespace drone::vtc {
namespace {
using Json = nlohmann::json;
/// 读取并解析 JSON 文件；失败抛异常并携带路径
Json ParseConfigFile(const std::string& path) {
std::ifstream ifs(path);
if (!ifs) {
throw std::runtime_error("配置文件打开失败: " + path);
}
try {
return Json::parse(ifs);
} catch (const std::exception& e) {
throw std::runtime_error("配置文件解析失败: " + path + " (" + e.what() + ")");
}
}
/// 取必需的对象分组；缺失或类型错抛异常
const Json& RequireSection(const Json& root, const char* section) {
const auto it = root.find(section);
if (it == root.end() || !it->is_object()) {
throw std::runtime_error(std::string("配置缺失分组或类型错误: ") + section);
}
return *it;
}
/// 取必需的数值字段；缺失/类型错抛异常并携带字段名
double RequireNumber(const Json& section, const char* section_name, const char* key) {
const auto it = section.find(key);
if (it == section.end() || !it->is_number()) {
throw std::runtime_error(std::string("配置缺失字段或类型错误: ") +
section_name + "." + key);
}
return it->get<double>();
}
/// 取必需的整数字段
std::int64_t RequireInteger(const Json& section, const char* section_name,
const char* key) {
const auto it = section.find(key);
if (it == section.end() || !it->is_number_integer()) {
throw std::runtime_error(std::string("配置缺失字段或类型错误(需整数): ") +
section_name + "." + key);
}
return it->get<std::int64_t>();
}
/// 取必需的字符串字段
std::string RequireString(const Json& section, const char* section_name,
const char* key) {
const auto it = section.find(key);
if (it == section.end() || !it->is_string()) {
throw std::runtime_error(std::string("配置缺失字段或类型错误(需字符串): ") +
section_name + "." + key);
}
return it->get<std::string>();
}
/// 校验字段为正数；非法抛异常并携带字段名
double RequirePositive(double value, const char* field_name) {
if (!(value > 0.0)) {
throw std::runtime_error(std::string("配置非法(必须为正): ") + field_name);
}
return value;
}
/// 校验字段为非负数
double RequireNonNegative(double value, const char* field_name) {
if (value < 0.0) {
throw std::runtime_error(std::string("配置非法(不得为负): ") + field_name);
}
return value;
}
/// 校验字段在开区间 (lo, hi)
double RequireRange(double value, double lo, double hi, const char* field_name) {
if (!(value > lo && value < hi)) {
throw std::runtime_error(std::string("配置非法(超出范围): ") + field_name);
}
return value;
}
ControlGroupConfig LoadControl(const Json& root) {
const Json& s = RequireSection(root, "control");
ControlGroupConfig c;
c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"),
"control.frequency_hz");
c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms");
c.radar_stale_ms = RequireInteger(s, "control", "radar_stale_ms");
c.attitude_stale_ms = RequireInteger(s, "control", "attitude_stale_ms");
if (c.visual_stale_ms <= 0 || c.radar_stale_ms <= 0 || c.attitude_stale_ms <= 0) {
throw std::runtime_error("配置非法: control.*_stale_ms 必须为正");
}
return c;
}
CameraGroupConfig LoadCamera(const Json& root) {
const Json& s = RequireSection(root, "camera");
CameraGroupConfig c;
c.image_width = static_cast<int>(
RequirePositive(RequireInteger(s, "camera", "image_width"), "camera.image_width"));
c.image_height = static_cast<int>(
RequirePositive(RequireInteger(s, "camera", "image_height"), "camera.image_height"));
c.fov_h_deg = RequireRange(RequireNumber(s, "camera", "fov_h_deg"), 0.0, 180.0,
"camera.fov_h_deg");
c.fov_v_deg = RequireRange(RequireNumber(s, "camera", "fov_v_deg"), 0.0, 180.0,
"camera.fov_v_deg");
return c;
}
HeadingGroupConfig LoadHeading(const Json& root) {
const Json& s = RequireSection(root, "heading");
HeadingGroupConfig h;
const std::string mode = RequireString(s, "heading", "mode");
if (mode == "position") {
h.mode = HeadingMode::kPosition;
} else if (mode == "rate") {
h.mode = HeadingMode::kRate;
} else {
throw std::runtime_error("配置非法: heading.mode 仅支持 position / rate");
}
h.gain = RequireNonNegative(RequireNumber(s, "heading", "gain"), "heading.gain");
h.yaw_rate_limit_dps = RequirePositive(
RequireNumber(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps");
h.yaw_slew_limit_dps2 = RequirePositive(
RequireNumber(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2");
return h;
}
VerticalGroupConfig LoadVertical(const Json& root) {
const Json& s = RequireSection(root, "vertical");
VerticalGroupConfig v;
v.vz_gain_mps_per_deg = RequireNonNegative(
RequireNumber(s, "vertical", "vz_gain_mps_per_deg"), "vertical.vz_gain_mps_per_deg");
v.vz_limit_mps = RequirePositive(RequireNumber(s, "vertical", "vz_limit_mps"),
"vertical.vz_limit_mps");
return v;
}
DistanceGroupConfig LoadDistance(const Json& root) {
const Json& s = RequireSection(root, "distance");
DistanceGroupConfig d;
d.d_exp_m = RequirePositive(RequireNumber(s, "distance", "d_exp_m"), "distance.d_exp_m");
d.kp = RequireNonNegative(RequireNumber(s, "distance", "kp"), "distance.kp");
d.ki = RequireNonNegative(RequireNumber(s, "distance", "ki"), "distance.ki");
d.kd = RequireNonNegative(RequireNumber(s, "distance", "kd"), "distance.kd");
d.integral_limit = RequireNonNegative(RequireNumber(s, "distance", "integral_limit"),
"distance.integral_limit");
d.derivative_filter_coef = RequireRange(
RequireNumber(s, "distance", "derivative_filter_coef"), -0.000001, 1.0,
"distance.derivative_filter_coef");
if (d.derivative_filter_coef < 0.0) {
d.derivative_filter_coef = 0.0; // 容许 -0.0
}
d.approach_velocity_limit_mps = RequirePositive(
RequireNumber(s, "distance", "approach_velocity_limit_mps"),
"distance.approach_velocity_limit_mps");
d.retreat_velocity_limit_mps = RequirePositive(
RequireNumber(s, "distance", "retreat_velocity_limit_mps"),
"distance.retreat_velocity_limit_mps");
const std::string action = RequireString(s, "distance", "no_distance_action");
if (action == "hold") {
d.no_distance_action = NoDistanceAction::kHold;
} else if (action == "slow_approach") {
d.no_distance_action = NoDistanceAction::kSlowApproach;
} else if (action == "exit") {
d.no_distance_action = NoDistanceAction::kExit;
} else {
throw std::runtime_error(
"配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit");
}
d.no_distance_approach_limit_mps = RequireNonNegative(
RequireNumber(s, "distance", "no_distance_approach_limit_mps"),
"distance.no_distance_approach_limit_mps");
return d;
}
AccelLimitConfig LoadAccelLimit(const Json& root) {
const Json& s = RequireSection(root, "accel_limit");
AccelLimitConfig a;
a.ax_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ax_mps2"),
"accel_limit.ax_mps2");
a.ay_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ay_mps2"),
"accel_limit.ay_mps2");
a.az_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "az_mps2"),
"accel_limit.az_mps2");
return a;
}
} // namespace
TrackingConfig TrackingConfig::LoadFromJson(const std::string& path) {
const Json root = ParseConfigFile(path);
TrackingConfig cfg;
cfg.control = LoadControl(root);
cfg.camera = LoadCamera(root);
cfg.heading = LoadHeading(root);
cfg.vertical = LoadVertical(root);
cfg.distance = LoadDistance(root);
cfg.accel_limit = LoadAccelLimit(root);
return cfg;
}
} // namespace drone::vtc
reference-c++/visual_track_control/tests/frame_transform_test.cpp
+56
View File
@@ -0,0 +1,56 @@
// =============================================================================
// frame_transform_test.cpp —— RotateBodyToNed 单元测试
//
// 覆盖（物理追踪思路 §2.3，A4）：
// 零航向恒等、朝东时机体前向 → 东向、垂直分量不变、模长守恒。
// =============================================================================
#include <cmath>
#include <gtest/gtest.h>
#include "visual_track_control/frame_transform.h"
namespace {
using drone::vtc::RotateBodyToNed;
constexpr double kPi = 3.14159265358979323846;
TEST(FrameTransformTest, ZeroYawIsIdentity) {
double x = 1.0, y = 0.5, z = -2.0;
RotateBodyToNed(x, y, z, 0.0);
EXPECT_NEAR(x, 1.0, 1e-12); // 北 = 前
EXPECT_NEAR(y, 0.5, 1e-12); // 东 = 右
EXPECT_NEAR(z, -2.0, 1e-12);
}
TEST(FrameTransformTest, YawEast90MapsForwardToEast) {
double x = 2.0, y = 0.0, z = 0.0;
RotateBodyToNed(x, y, z, kPi / 2.0); // 机头朝东
EXPECT_NEAR(x, 0.0, 1e-9); // 北向 ≈ 0
EXPECT_NEAR(y, 2.0, 1e-9); // 东向 = 前向速度
}
TEST(FrameTransformTest, RightIsEastWhenHeadingNorth) {
double x = 0.0, y = 1.0, z = 0.0;
RotateBodyToNed(x, y, z, 0.0);
EXPECT_NEAR(x, 0.0, 1e-12);
EXPECT_NEAR(y, 1.0, 1e-12);
}
TEST(FrameTransformTest, DownComponentUnchanged) {
double x = 1.0, y = 1.0, z = 3.5;
RotateBodyToNed(x, y, z, kPi / 3.0);
EXPECT_DOUBLE_EQ(z, 3.5);
}
TEST(FrameTransformTest, PreservesHorizontalNorm) {
// 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
double x = 3.0, y = -4.0, z = 1.0;
const double norm_before = std::hypot(x, y);
RotateBodyToNed(x, y, z, 0.7);
EXPECT_NEAR(std::hypot(x, y), norm_before, 1e-9);
}
} // namespace
reference-c++/visual_track_control/tests/line_of_sight_test.cpp
+69
View File
@@ -0,0 +1,69 @@
// =============================================================================
// line_of_sight_test.cpp —— PixelToAngle 单元测试
//
// 覆盖（物理追踪思路 §4.1 / §5.1，C2）：
// 零偏移、画面边缘偏移 = 半视场角、奇对称、单调性、
// 与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
// =============================================================================
#include <cmath>
#include <gtest/gtest.h>
#include "visual_track_control/line_of_sight.h"
namespace {
using drone::vtc::PixelToAngle;
constexpr double kPi = 3.14159265358979323846;
constexpr double kRadToDeg = 180.0 / kPi;
TEST(PixelToAngleTest, ZeroOffsetGivesZeroAngle) {
PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
EXPECT_DOUBLE_EQ(conv.Convert(0.0), 0.0);
}
TEST(PixelToAngleTest, HalfSizeOffsetGivesHalfFov) {
// 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2
PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
EXPECT_NEAR(conv.Convert(500.0), 45.0 * kPi / 180.0, 1e-12);
EXPECT_NEAR(conv.Convert(-500.0), -45.0 * kPi / 180.0, 1e-12);
}
TEST(PixelToAngleTest, OddSymmetry) {
PixelToAngle conv(1920.0, 60.0 * kPi / 180.0);
for (const double offset : {1.0, 100.0, 730.5}) {
EXPECT_NEAR(conv.Convert(-offset), -conv.Convert(offset), 1e-15);
}
}
TEST(PixelToAngleTest, MonotonicIncreasing) {
PixelToAngle conv(1080.0, 35.0 * kPi / 180.0);
double prev = conv.Convert(-600.0);
for (double x = -500.0; x <= 600.0; x += 100.0) {
const double cur = conv.Convert(x);
EXPECT_GT(cur, prev);
prev = cur;
}
}
TEST(PixelToAngleTest, DeviatesFromLinearApproximation) {
// C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°，
// 针孔结果必须与之明显偏离（证明未走线性支路）。
PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
const double pinhole_deg = conv.Convert(250.0) * kRadToDeg; // atan(250·2·tan45°/1000)
const double linear_deg = 250.0 * (90.0 / 1000.0); // θ·offset/size = 22.5°
EXPECT_NEAR(pinhole_deg, std::atan(0.5) * kRadToDeg, 1e-9); // 26.565°
EXPECT_GT(std::fabs(pinhole_deg - linear_deg), 2.0);
}
TEST(PixelToAngleTest, ConstructorRejectsInvalidArgs) {
EXPECT_THROW(PixelToAngle(0.0, 1.0), std::invalid_argument);
EXPECT_THROW(PixelToAngle(-10.0, 1.0), std::invalid_argument);
EXPECT_THROW(PixelToAngle(1000.0, 0.0), std::invalid_argument);
EXPECT_THROW(PixelToAngle(1000.0, kPi), std::invalid_argument);
EXPECT_THROW(PixelToAngle(1000.0, kPi + 0.1), std::invalid_argument);
}
} // namespace
reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp
+181
View File
@@ -0,0 +1,181 @@
// =============================================================================
// mavlink_command_gateway_test.cpp —— MavlinkCommandGateway 单元测试
//
// 覆盖（物理追踪思路 §2.2、§4.2，A2 掩码三分支）：
// ID 84 速度模式的 type_mask 选择（角速率非零 → 速率掩码；航向角非零 →
// 航向掩码；两者均零 → 速率掩码配零角速率，保持航向）、
// 速度/航向字段编码、刹车消息、坐标基准、传输失败传播。
//
// 构建约束：本文件**不得**包含 common/mavlink.h（MAVLINK_HELPER 默认空宏，
// 多 TU 包含会多重定义），故按 MAVLink v2 线格式手工解码：
// 帧头 10 字节（STX=0xFD, len, flags×2, seq, sys, comp, msgid×3 LE），
// 载荷按字段尺寸降序排列（ID 84：time,x,y,z,vx,vy,vz,afx,afy,afz,
// yaw,yaw_rate 均 4 字节在前，随后 u16 type_mask，再 target_system、
// target_component、coordinate_frame 三个 u8）。
// =============================================================================
#include <cstdint>
#include <cstring>
#include <vector>
#include <gtest/gtest.h>
#include "visual_track_control/i_command_transport.h"
#include "visual_track_control/mavlink_command_gateway.h"
#include "visual_track_control/vtc_types.h"
namespace {
using drone::vtc::ControlOutput;
using drone::vtc::ICommandTransport;
using drone::vtc::MavlinkCommandGateway;
// ---- MAVLink v2 帧布局常量（见文件头说明） ----
constexpr std::uint8_t kMavlinkV2Stx = 0xFD;
constexpr std::size_t kHeaderLen = 10;
constexpr std::size_t kMsgId84PayloadLen = 53;
// ---- ID 84 载荷字段偏移（相对载荷起点） ----
constexpr std::size_t kOffVx = 16;
constexpr std::size_t kOffVy = 20;
constexpr std::size_t kOffVz = 24;
constexpr std::size_t kOffYaw = 40;
constexpr std::size_t kOffYawRate = 44;
constexpr std::size_t kOffTypeMask = 48;
// u8 三字段线序（生成头 mavlink_msg_set_position_target_local_ned.h 为准）
constexpr std::size_t kOffTargetSystem = 50;
constexpr std::size_t kOffTargetComponent = 51;
constexpr std::size_t kOffCoordFrame = 52;
// ---- 预期掩码值（与实现中常量一致：置位 = 忽略） ----
// 基础 0x01C7（忽略位置与加速度）| 航向相关位
constexpr std::uint16_t kExpectMaskYaw = 0x01C7 | 0x0800; // 忽略 yaw_rate
constexpr std::uint16_t kExpectMaskYawRate = 0x01C7 | 0x0400; // 忽略 yaw
constexpr std::uint16_t kExpectMaskBrake = 0x01C7 | 0x0C00; // 两者均忽略
/// 捕获式传输：记录最后一次发送的字节流，可注入失败
class CaptureTransport : public ICommandTransport {
public:
bool Send(const std::uint8_t* data, std::size_t len) override {
++send_count_;
last_.assign(data, data + len);
return succeed_;
}
void SetSucceed(bool ok) { succeed_ = ok; }
int send_count() const { return send_count_; }
const std::vector<std::uint8_t>& last() const { return last_; }
private:
bool succeed_ = true;
int send_count_ = 0;
std::vector<std::uint8_t> last_;
};
float ReadFloat(const std::vector<std::uint8_t>& buf, std::size_t payload_off) {
float v;
std::memcpy(&v, buf.data() + kHeaderLen + payload_off, sizeof(v));
return v;
}
std::uint16_t ReadU16(const std::vector<std::uint8_t>& buf, std::size_t payload_off) {
return static_cast<std::uint16_t>(buf[kHeaderLen + payload_off]) |
(static_cast<std::uint16_t>(buf[kHeaderLen + payload_off + 1]) << 8);
}
void ExpectId84Header(const std::vector<std::uint8_t>& buf) {
ASSERT_GE(buf.size(), kHeaderLen + kMsgId84PayloadLen + 2); // +CRC
EXPECT_EQ(buf[0], kMavlinkV2Stx);
EXPECT_EQ(buf[1], kMsgId84PayloadLen);
// msgid 24 位小端（字节 7-9）= 84
EXPECT_EQ(buf[7], 84);
EXPECT_EQ(buf[8], 0);
EXPECT_EQ(buf[9], 0);
}
constexpr double kDegToRad = 3.14159265358979323846 / 180.0;
TEST(MavlinkGatewayTest, YawRateModeSelectsRateMask) {
CaptureTransport transport;
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
ControlOutput out;
out.mode = drone::vtc::ControlMode::kVelocityHeading;
out.vx_mps = 1.5;
out.vy_mps = -0.5;
out.vz_mps = 0.25;
out.yaw_rate_dps = 5.0;
out.yaw_deg = 0.0;
ASSERT_TRUE(gateway.SendVelocityHeading(out, 12345));
const auto& buf = transport.last();
ExpectId84Header(buf);
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 1.5F);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), -0.5F);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.25F);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), static_cast<float>(5.0 * kDegToRad));
EXPECT_EQ(buf[kHeaderLen + kOffCoordFrame], 1); // MAV_FRAME_LOCAL_NED
EXPECT_EQ(buf[kHeaderLen + kOffTargetSystem], 3); // target_system
EXPECT_EQ(buf[kHeaderLen + kOffTargetComponent], 4); // target_component
}
TEST(MavlinkGatewayTest, YawOnlySelectsYawMask) {
CaptureTransport transport;
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
ControlOutput out;
out.mode = drone::vtc::ControlMode::kVelocityHeading;
out.yaw_deg = 10.0;
out.yaw_rate_dps = 0.0;
ASSERT_TRUE(gateway.SendVelocityHeading(out, 1));
const auto& buf = transport.last();
ExpectId84Header(buf);
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYaw);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYaw), static_cast<float>(10.0 * kDegToRad));
}
TEST(MavlinkGatewayTest, BothZeroKeepsHeadingViaRateMask) {
// A2 回归：旧实现在零角速率时落到航向掩码，等于误发绝对航向 0（正北）。
// 正确行为：速率掩码 + 零角速率 = 保持当前航向。
CaptureTransport transport;
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
ControlOutput out;
out.mode = drone::vtc::ControlMode::kVelocityHeading;
out.vx_mps = 1.0;
ASSERT_TRUE(gateway.SendVelocityHeading(out, 1));
const auto& buf = transport.last();
ExpectId84Header(buf);
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), 0.0F);
}
TEST(MavlinkGatewayTest, BrakeHoverSendsZeroVelocityAllIgnored) {
CaptureTransport transport;
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
ASSERT_TRUE(gateway.SendBrakeHover(777));
const auto& buf = transport.last();
ExpectId84Header(buf);
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskBrake);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 0.0F);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), 0.0F);
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.0F);
}
TEST(MavlinkGatewayTest, TransportFailurePropagates) {
CaptureTransport transport;
transport.SetSucceed(false);
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
ControlOutput out;
out.mode = drone::vtc::ControlMode::kVelocityHeading;
EXPECT_FALSE(gateway.SendVelocityHeading(out, 1));
EXPECT_FALSE(gateway.SendBrakeHover(1));
EXPECT_EQ(transport.send_count(), 2);
}
} // namespace
reference-c++/visual_track_control/tests/pid_controller_test.cpp
+123
View File
@@ -0,0 +1,123 @@
// =============================================================================
// pid_controller_test.cpp —— PidController 单元测试
//
// 覆盖（物理追踪思路 §6.2 / §6.4）：
// 位置式 P/I/D 三项、积分钳位、输出限幅、异常节拍保护、
// anti-windup 条件积分（C1 修复回归：饱和冻结 + 误差反号回卷）、
// ResetIntegral / Reset 行为。
// =============================================================================
#include <gtest/gtest.h>
#include "visual_track_control/pid_controller.h"
namespace {
using drone::vtc::PidController;
using drone::vtc::PidParams;
PidParams MakeParams(double kp, double ki, double kd, double integral_limit,
double output_limit, double filter_coef = 0.0) {
PidParams p;
p.kp = kp;
p.ki = ki;
p.kd = kd;
p.integral_limit = integral_limit;
p.derivative_filter_coef = filter_coef;
p.output_limit = output_limit;
return p;
}
TEST(PidControllerTest, ProportionalOnly) {
PidController pid(MakeParams(2.0, 0.0, 0.0, 0.0, 0.0));
EXPECT_DOUBLE_EQ(pid.Update(3.0, 0.1), 6.0);
EXPECT_DOUBLE_EQ(pid.Update(-1.5, 0.1), -3.0);
}
TEST(PidControllerTest, IntegralAccumulates) {
PidController pid(MakeParams(0.0, 1.0, 0.0, 0.0, 0.0));
// 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 1.0);
EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 2.0);
}
TEST(PidControllerTest, IntegralClampedToLimit) {
PidController pid(MakeParams(0.0, 1.0, 0.0, /*integral_limit=*/1.0, 0.0));
for (int i = 0; i < 5; ++i) {
pid.Update(10.0, 1.0);
}
// 积分候选值被钳位在 1.0 → 输出恒为 1.0
EXPECT_DOUBLE_EQ(pid.LastOutput(), 1.0);
}
TEST(PidControllerTest, OutputClampedToLimit) {
PidController pid(MakeParams(10.0, 0.0, 0.0, 0.0, /*output_limit=*/3.0));
EXPECT_DOUBLE_EQ(pid.Update(1.0, 0.1), 3.0);
EXPECT_DOUBLE_EQ(pid.Update(-1.0, 0.1), -3.0);
}
TEST(PidControllerTest, AntiWindupFreezeWhileSaturatedSameDirection) {
// kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
// 输出恒钉在 +5（10 拍累计 10 拍也不会让积分继续增长）。
PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0));
for (int i = 0; i < 10; ++i) {
EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0);
}
}
TEST(PidControllerTest, AntiWindupUnwindsAfterErrorFlips) {
// C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
// 冻结回卷、输出一直钉在 +5）。
// kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
// error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
// 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0));
for (int i = 0; i < 3; ++i) {
EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0);
}
EXPECT_DOUBLE_EQ(pid.Update(-1.0, 1.0), -2.0); // 首拍立即回卷
double out = -2.0;
for (int i = 1; i < 12; ++i) {
out = pid.Update(-1.0, 1.0);
}
EXPECT_NEAR(out, -5.0, 1e-9); // 旧判据此处仍为 +5.0
}
TEST(PidControllerTest, DerivativeFirstStepIsZero) {
PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0));
// 首拍无微分历史：微分项为 0
EXPECT_DOUBLE_EQ(pid.Update(5.0, 1.0), 0.0);
}
TEST(PidControllerTest, DerivativeWithLowPassFilter) {
PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0, /*filter=*/0.5));
pid.Update(0.0, 1.0); // 建立微分历史（导数 0）
// 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
EXPECT_DOUBLE_EQ(pid.Update(1.0, 1.0), 0.5);
}
TEST(PidControllerTest, ZeroOrNegativeDtHoldsLastOutput) {
PidController pid(MakeParams(1.0, 0.0, 0.0, 0.0, 0.0));
EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.1), 2.0);
EXPECT_DOUBLE_EQ(pid.Update(99.0, 0.0), 2.0);
EXPECT_DOUBLE_EQ(pid.Update(99.0, -1.0), 2.0);
}
TEST(PidControllerTest, ResetIntegralClearsOnlyIntegral) {
PidController pid(MakeParams(0.0, 1.0, 1.0, 0.0, 0.0));
pid.Update(2.0, 1.0); // 积分 = 2
pid.ResetIntegral();
// 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
EXPECT_DOUBLE_EQ(pid.Update(0.0, 1.0), -2.0);
}
TEST(PidControllerTest, ResetClearsEverything) {
PidController pid(MakeParams(1.0, 1.0, 1.0, 0.0, 0.0));
pid.Update(5.0, 1.0);
pid.Reset();
EXPECT_DOUBLE_EQ(pid.LastOutput(), 0.0);
// 全清后等同全新控制器：首拍无微分历史
EXPECT_DOUBLE_EQ(pid.Update(3.0, 1.0), 3.0 + 3.0); // kp·e + ki·∫e
}
} // namespace
reference-c++/visual_track_control/tests/rate_limiter_test.cpp
+76
View File
@@ -0,0 +1,76 @@
// =============================================================================
// rate_limiter_test.cpp —— Clamp / SlewRateLimiter 单元测试
//
// 覆盖（物理追踪思路 §8、状态机设计 S6）：
// 静态限幅、变化率限幅步进、首条指令从零渐变（S6）、
// 异常节拍保护、Reset 归零。
// =============================================================================
#include <gtest/gtest.h>
#include "visual_track_control/rate_limiter.h"
namespace {
using drone::vtc::Clamp;
using drone::vtc::SlewRateLimiter;
TEST(ClampTest, ClampsValue) {
EXPECT_DOUBLE_EQ(Clamp(5.0, -2.0, 2.0), 2.0);
EXPECT_DOUBLE_EQ(Clamp(-5.0, -2.0, 2.0), -2.0);
EXPECT_DOUBLE_EQ(Clamp(1.5, -2.0, 2.0), 1.5);
// 边界值：落在区间内
EXPECT_DOUBLE_EQ(Clamp(2.0, -2.0, 2.0), 2.0);
EXPECT_DOUBLE_EQ(Clamp(-2.0, -2.0, 2.0), -2.0);
}
TEST(SlewRateLimiterTest, FirstCommandRampsFromZero) {
// S6：初始输出为 0，首条大指令只能按变化率渐变
SlewRateLimiter lim(/*rate_limit=*/2.0);
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 1.0); // 0 + 2×0.5
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 2.0); // 继续渐变
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 10.0), 10.0); // 步长足够 → 到达目标
}
TEST(SlewRateLimiterTest, TracksTargetWithinStep) {
SlewRateLimiter lim(10.0);
// 步长 10×1=10，目标 3 在步长内 → 直接到达
EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0);
EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0); // 已在目标：保持
}
TEST(SlewRateLimiterTest, NegativeDirection) {
SlewRateLimiter lim(2.0);
EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -1.0);
EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -2.0);
}
TEST(SlewRateLimiterTest, ZeroOrNegativeDtHolds) {
SlewRateLimiter lim(2.0);
lim.Limit(10.0, 1.0); // 输出 2.0
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.0), 2.0);
EXPECT_DOUBLE_EQ(lim.Limit(10.0, -1.0), 2.0);
}
TEST(SlewRateLimiterTest, ZeroRateLimitFreezesOutput) {
SlewRateLimiter lim(0.0);
EXPECT_DOUBLE_EQ(lim.Limit(5.0, 1.0), 0.0);
}
TEST(SlewRateLimiterTest, ResetToValue) {
SlewRateLimiter lim(2.0);
lim.Limit(10.0, 1.0);
lim.Reset();
EXPECT_DOUBLE_EQ(lim.LastOutput(), 0.0);
lim.Reset(3.5);
EXPECT_DOUBLE_EQ(lim.LastOutput(), 3.5);
// 复位后从新值继续渐变
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 5.5); // 3.5 + 2×1
}
TEST(SlewRateLimiterTest, CustomInitialValue) {
SlewRateLimiter lim(1.0, /*initial=*/5.0);
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 6.0);
}
} // namespace
reference-c++/visual_track_control/tests/staleness_checker_test.cpp
+41
View File
@@ -0,0 +1,41 @@
// =============================================================================
// staleness_checker_test.cpp —— StalenessChecker 单元测试
//
// 覆盖（物理追踪思路 §7.1）：
// 从未取样判超龄、有效期内新鲜、超出有效期超龄、
// 边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
// =============================================================================
#include <gtest/gtest.h>
#include "visual_track_control/staleness_checker.h"
namespace {
using drone::vtc::StalenessChecker;
TEST(StalenessCheckerTest, NeverSampledIsStale) {
EXPECT_TRUE(StalenessChecker::IsStale(0, 1000, 200));
EXPECT_TRUE(StalenessChecker::IsStale(-5, 1000, 200));
}
TEST(StalenessCheckerTest, WithinValidityIsFresh) {
EXPECT_FALSE(StalenessChecker::IsStale(900, 1000, 200));
EXPECT_FALSE(StalenessChecker::IsStale(1000, 1000, 200)); // 刚取样
}
TEST(StalenessCheckerTest, BeyondValidityIsStale) {
EXPECT_TRUE(StalenessChecker::IsStale(799, 1000, 200)); // 201 > 200
}
TEST(StalenessCheckerTest, ExactlyAtBoundaryIsFresh) {
// 超龄判定为严格大于：恰好等于有效期仍算新鲜
EXPECT_FALSE(StalenessChecker::IsStale(800, 1000, 200));
}
TEST(StalenessCheckerTest, InconsistentTimestampIsStale) {
// C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信）
EXPECT_TRUE(StalenessChecker::IsStale(2000, 1000, 200));
}
} // namespace
reference-c++/visual_track_control/tests/tracking_control_law_test.cpp
+349
View File
@@ -0,0 +1,349 @@
// =============================================================================
// tracking_control_law_test.cpp —— TrackingControlLaw 主控律单元测试
//
// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、
// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
// =============================================================================
#include <cmath>
#include <gtest/gtest.h>
#include "visual_track_control/tracking_control_law.h"
#include "visual_track_control/vtc_config.h"
#include "visual_track_control/vtc_types.h"
namespace {
using namespace drone::vtc;
constexpr double kPi = 3.14159265358979323846;
constexpr double kDegToRad = kPi / 180.0;
constexpr double kRadToDeg = 180.0 / kPi;
/// 测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
/// 画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
/// 偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
TrackingConfig MakeConfig() {
TrackingConfig cfg;
cfg.control.frequency_hz = 20.0;
cfg.control.visual_stale_ms = 200;
cfg.control.radar_stale_ms = 500;
cfg.control.attitude_stale_ms = 300;
cfg.camera.image_width = 1000;
cfg.camera.image_height = 1000;
cfg.camera.fov_h_deg = 90.0;
cfg.camera.fov_v_deg = 90.0;
cfg.heading.mode = HeadingMode::kRate;
cfg.heading.gain = 1.0;
cfg.heading.yaw_rate_limit_dps = 30.0;
cfg.heading.yaw_slew_limit_dps2 = 100000.0;
cfg.vertical.vz_gain_mps_per_deg = 0.1;
cfg.vertical.vz_limit_mps = 2.0;
cfg.distance.d_exp_m = 10.0;
cfg.distance.kp = 0.5;
cfg.distance.ki = 0.0;
cfg.distance.kd = 0.0;
cfg.distance.integral_limit = 0.0;
cfg.distance.derivative_filter_coef = 0.0;
cfg.distance.approach_velocity_limit_mps = 3.0;
cfg.distance.retreat_velocity_limit_mps = 1.0;
cfg.distance.no_distance_action = NoDistanceAction::kHold;
cfg.distance.no_distance_approach_limit_mps = 0.5;
cfg.accel_limit.ax_mps2 = 1000.0;
cfg.accel_limit.ay_mps2 = 1000.0;
cfg.accel_limit.az_mps2 = 1000.0;
return cfg;
}
/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜
ControlSnapshot MakeSnapshot(std::int64_t now_ms) {
ControlSnapshot s;
s.track.tracked = true;
s.track.is_predicted = false;
s.track.center = {500.0, 500.0};
s.track_time_ms = now_ms;
s.attitude_present = true;
s.attitude_time_ms = now_ms;
return s;
}
// ---- 超龄降级（§7.1 / §9.2） ----
TEST(TrackingControlLawTest, AttitudeMissingDegrades) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.attitude_present = false;
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
EXPECT_FALSE(out.valid);
}
TEST(TrackingControlLawTest, AttitudeStaleDegrades) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.attitude_time_ms = 600; // 400 ms 前 > 300 ms 阈值
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
}
TEST(TrackingControlLawTest, VisionStaleDegrades) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track_time_ms = 700; // 300 ms 前 > 200 ms 阈值
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale);
}
TEST(TrackingControlLawTest, NeverSampledTrackDegrades) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track_time_ms = 0; // 从未取样
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale);
}
TEST(TrackingControlLawTest, AttitudeStaleBeatsVisionStale) {
// 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.attitude_present = false;
snap.track_time_ms = 0;
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
}
// ---- 判丢刹车（§6.4） ----
TEST(TrackingControlLawTest, LostTargetBrakes) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track.tracked = false;
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kBrakeHover);
EXPECT_TRUE(out.valid);
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
EXPECT_DOUBLE_EQ(out.vy_mps, 0.0);
EXPECT_DOUBLE_EQ(out.vz_mps, 0.0);
}
// ---- Coast（§6.4） ----
TEST(TrackingControlLawTest, CoastSetsFlagAndKeepsTracking) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track.is_predicted = true;
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
EXPECT_TRUE(out.coast_active);
EXPECT_TRUE(out.valid);
}
// ---- 水平航向通道（§4.2，A2） ----
TEST(TrackingControlLawTest, RateModeMapsBetaToYawRate) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track.center = {600.0, 500.0}; // 目标偏右 100 像素
const auto out = law.Update(snap, 1000);
const double beta_deg = std::atan(0.2) * kRadToDeg; // ≈ 11.31°
EXPECT_NEAR(out.yaw_rate_dps, beta_deg, 1e-6); // gain=1，右转正
EXPECT_DOUBLE_EQ(out.yaw_deg, 0.0);
}
TEST(TrackingControlLawTest, RateModeClampsToYawRateLimit) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track.center = {950.0, 500.0}; // 大偏移：β≈43.5° > 30°/s 上限
const auto out = law.Update(snap, 1000);
EXPECT_NEAR(out.yaw_rate_dps, 30.0, 1e-9);
}
TEST(TrackingControlLawTest, PositionModeComposesAbsoluteHeading) {
// A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
auto cfg = MakeConfig();
cfg.heading.mode = HeadingMode::kPosition;
TrackingControlLaw law(cfg);
auto snap = MakeSnapshot(1000);
snap.track.center = {600.0, 500.0};
snap.yaw_rad = 45.0 * kDegToRad;
const auto out = law.Update(snap, 1000);
const double beta_deg = std::atan(0.2) * kRadToDeg;
EXPECT_NEAR(out.yaw_deg, 45.0 + beta_deg, 1e-6);
EXPECT_DOUBLE_EQ(out.yaw_rate_dps, 0.0);
}
TEST(TrackingControlLawTest, PositionModeWrapsAround180) {
auto cfg = MakeConfig();
cfg.heading.mode = HeadingMode::kPosition;
TrackingControlLaw law(cfg);
auto snap = MakeSnapshot(1000);
snap.track.center = {600.0, 500.0};
snap.yaw_rad = 175.0 * kDegToRad;
const auto out = law.Update(snap, 1000);
const double beta_deg = std::atan(0.2) * kRadToDeg;
// 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
EXPECT_NEAR(out.yaw_deg, 175.0 + beta_deg - 360.0, 1e-6);
EXPECT_GT(out.yaw_deg, -180.0);
EXPECT_LT(out.yaw_deg, 180.0);
}
// ---- 垂直统一速度支路（§5，A1） ----
TEST(TrackingControlLawTest, VerticalOffsetDrivesVz) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track.center = {500.0, 600.0}; // 目标偏低 100 像素 → α>0 → 下降
const auto out = law.Update(snap, 1000);
const double alpha_deg = std::atan(0.2) * kRadToDeg;
EXPECT_NEAR(out.vz_mps, 0.1 * alpha_deg, 1e-6);
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); // A1：不切姿态微调模式
}
TEST(TrackingControlLawTest, VerticalVelocityClamped) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.track.center = {500.0, 990.0}; // α≈44.5° × 0.1 = 4.45 > 2 上限
const auto out = law.Update(snap, 1000);
EXPECT_NEAR(out.vz_mps, 2.0, 1e-9);
}
// ---- 距离通道（§6.2 / §6.3） ----
TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.radar_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3
snap.associated_to_target = true;
snap.radar_time_ms = 1000;
const auto out = law.Update(snap, 1000);
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
}
TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.radar_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
snap.associated_to_target = true;
snap.radar_time_ms = 1000;
const auto out = law.Update(snap, 1000);
EXPECT_NEAR(out.vx_mps, -1.0, 1e-9);
}
TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) {
TrackingControlLaw law(MakeConfig()); // 缺省 kHold
auto snap = MakeSnapshot(1000);
snap.associated_to_target = false;
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
}
TEST(TrackingControlLawTest, NoDistanceSlowApproach) {
auto cfg = MakeConfig();
cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach;
TrackingControlLaw law(cfg);
auto snap = MakeSnapshot(1000);
snap.associated_to_target = false;
const auto out = law.Update(snap, 1000);
EXPECT_NEAR(out.vx_mps, 0.5, 1e-9);
}
TEST(TrackingControlLawTest, NoDistanceExitDegrades) {
auto cfg = MakeConfig();
cfg.distance.no_distance_action = NoDistanceAction::kExit;
TrackingControlLaw law(cfg);
auto snap = MakeSnapshot(1000);
snap.associated_to_target = false;
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit);
EXPECT_FALSE(out.valid);
}
TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) {
TrackingControlLaw law(MakeConfig()); // kHold
auto snap = MakeSnapshot(1000);
snap.radar_distance_m = 30.0;
snap.associated_to_target = true;
snap.radar_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用
const auto out = law.Update(snap, 1000);
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
}
// ---- 首拍/限幅（§8、S6） ----
TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) {
// S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
auto cfg = MakeConfig();
cfg.accel_limit.ax_mps2 = 2.0;
TrackingControlLaw law(cfg);
auto snap = MakeSnapshot(1000);
snap.radar_distance_m = 30.0; // 通道输出本应为 3.0
snap.associated_to_target = true;
snap.radar_time_ms = 1000;
const auto out = law.Update(snap, 1000); // 首拍：dt 按 1/20 Hz 兜底
EXPECT_NEAR(out.vx_mps, 2.0 * (1.0 / 20.0), 1e-9); // 0.1
}
// ---- 机体系 → NED 旋转（§2.3，A4） ----
TEST(TrackingControlLawTest, OutputRotatedToNedByYaw) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.yaw_rad = kPi / 2.0; // 机头朝东
snap.radar_distance_m = 30.0;
snap.associated_to_target = true;
snap.radar_time_ms = 1000;
const auto out = law.Update(snap, 1000);
// 机体前向 3.0 → 东向 3.0，北向 ≈ 0
EXPECT_NEAR(out.vx_mps, 0.0, 1e-9);
EXPECT_NEAR(out.vy_mps, 3.0, 1e-9);
}
TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) {
TrackingControlLaw law(MakeConfig());
auto snap = MakeSnapshot(1000);
snap.yaw_rad = 0.0;
snap.radar_distance_m = 30.0;
snap.associated_to_target = true;
snap.radar_time_ms = 1000;
const auto out = law.Update(snap, 1000);
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
EXPECT_NEAR(out.vy_mps, 0.0, 1e-9);
}
// ---- Reset（§6.4） ----
TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) {
auto cfg = MakeConfig();
cfg.accel_limit.ax_mps2 = 2.0;
TrackingControlLaw law(cfg);
auto snap = MakeSnapshot(1000);
snap.radar_distance_m = 30.0;
snap.associated_to_target = true;
snap.radar_time_ms = 1000;
for (std::int64_t t = 1000; t < 3000; t += 50) {
snap.track_time_ms = t;
snap.radar_time_ms = t;
snap.attitude_time_ms = t;
law.Update(snap, t); // 若干拍后 vx 已爬升到限幅 3.0
}
law.Reset();
snap.track_time_ms = 3000;
snap.radar_time_ms = 3000;
snap.attitude_time_ms = 3000;
const auto out = law.Update(snap, 3000);
// Reset 后限幅器归零：又从 0.1 开始渐变
EXPECT_NEAR(out.vx_mps, 0.1, 1e-9);
}
} // namespace
reference-c++/visual_track_control/tests/vtc_config_test.cpp
+224
View File
@@ -0,0 +1,224 @@
// =============================================================================
// vtc_config_test.cpp —— TrackingConfig::LoadFromJson 单元测试
//
// 覆盖（物理追踪思路 §8、需求分析 NFR-007）：
// 真实配置文件加载成功与取值正确（含 A1/A2 修改后的字段结构）、
// 缺字段 / 缺分组 / 非法枚举 / 非法数值 / 文件缺失均抛带字段名的异常。
//
// 依赖编译期宏：
// VTC_TEST_CONFIG —— config/track_control.json 绝对路径
// VTC_TEST_TMP_DIR —— 测试临时文件输出目录（构建目录）
// =============================================================================
#include <cstdio>
#include <fstream>
#include <stdexcept>
#include <string>
#include <gtest/gtest.h>
#include "visual_track_control/vtc_config.h"
namespace {
using drone::vtc::HeadingMode;
using drone::vtc::NoDistanceAction;
using drone::vtc::TrackingConfig;
/// 与真实配置同构的完整合法 JSON（用于派生各非法变体）
const char* kFullJson = R"({
"control": {
"frequency_hz": 20.0,
"visual_stale_ms": 200,
"radar_stale_ms": 500,
"attitude_stale_ms": 300
},
"camera": {
"image_width": 1920,
"image_height": 1080,
"fov_h_deg": 60.0,
"fov_v_deg": 35.0
},
"heading": {
"mode": "rate",
"gain": 1.0,
"yaw_rate_limit_dps": 30.0,
"yaw_slew_limit_dps2": 60.0
},
"vertical": {
"vz_gain_mps_per_deg": 0.1,
"vz_limit_mps": 2.0
},
"distance": {
"d_exp_m": 10.0,
"kp": 0.5,
"ki": 0.05,
"kd": 0.1,
"integral_limit": 2.0,
"derivative_filter_coef": 0.2,
"approach_velocity_limit_mps": 3.0,
"retreat_velocity_limit_mps": 1.0,
"no_distance_action": "hold",
"no_distance_approach_limit_mps": 0.5
},
"accel_limit": {
"ax_mps2": 2.0,
"ay_mps2": 2.0,
"az_mps2": 1.5
}
})";
/// 在 json 中把首个 from 替换为 to（未找到即测试失败）
void ReplaceOnce(std::string& json, const std::string& from, const std::string& to) {
const auto pos = json.find(from);
ASSERT_NE(pos, std::string::npos) << "未找到待替换片段: " << from;
json.replace(pos, from.size(), to);
}
/// 把 JSON 文本写入临时文件并返回路径（调用方用完后删除）
std::string WriteTempJson(const std::string& content, const char* name) {
const std::string path = std::string(VTC_TEST_TMP_DIR) + "/" + name;
std::ofstream ofs(path);
EXPECT_TRUE(static_cast<bool>(ofs)) << "无法写入临时配置: " << path;
ofs << content;
return path;
}
TEST(VtcConfigTest, LoadRealConfigSucceeds) {
const TrackingConfig cfg = TrackingConfig::LoadFromJson(VTC_TEST_CONFIG);
EXPECT_DOUBLE_EQ(cfg.control.frequency_hz, 20.0);
EXPECT_EQ(cfg.control.visual_stale_ms, 200);
EXPECT_EQ(cfg.camera.image_width, 1920);
EXPECT_EQ(cfg.camera.image_height, 1080);
EXPECT_DOUBLE_EQ(cfg.camera.fov_h_deg, 60.0);
EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate); // A2：默认速率式
EXPECT_DOUBLE_EQ(cfg.heading.yaw_rate_limit_dps, 30.0);
EXPECT_DOUBLE_EQ(cfg.vertical.vz_gain_mps_per_deg, 0.1);
EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0);
EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.0);
EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kHold);
EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0);
}
TEST(VtcConfigTest, FullJsonRoundTripMatchesExpected) {
// 内嵌合法 JSON 与真实配置文件行为一致
const std::string path = WriteTempJson(kFullJson, "vtc_cfg_full.json");
const TrackingConfig cfg = TrackingConfig::LoadFromJson(path);
EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate);
EXPECT_DOUBLE_EQ(cfg.distance.kp, 0.5);
std::remove(path.c_str());
}
TEST(VtcConfigTest, PositionModeAccepted) {
std::string json = kFullJson;
ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"position\"");
const std::string path = WriteTempJson(json, "vtc_cfg_position.json");
const TrackingConfig cfg = TrackingConfig::LoadFromJson(path);
EXPECT_EQ(cfg.heading.mode, HeadingMode::kPosition);
std::remove(path.c_str());
}
TEST(VtcConfigTest, MissingFileThrowsWithPath) {
try {
TrackingConfig::LoadFromJson("/nonexistent/vtc_no_such_file.json");
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("vtc_no_such_file.json"),
std::string::npos);
}
}
TEST(VtcConfigTest, MalformedJsonThrows) {
const std::string path = WriteTempJson("{ this is not json", "vtc_cfg_bad.json");
EXPECT_THROW(TrackingConfig::LoadFromJson(path), std::runtime_error);
std::remove(path.c_str());
}
TEST(VtcConfigTest, MissingFieldThrowsWithFieldName) {
std::string json = kFullJson;
ReplaceOnce(json, "\"gain\": 1.0,\n", "");
const std::string path = WriteTempJson(json, "vtc_cfg_missing.json");
try {
TrackingConfig::LoadFromJson(path);
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("heading.gain"), std::string::npos);
}
std::remove(path.c_str());
}
TEST(VtcConfigTest, MissingSectionThrows) {
std::string json = kFullJson;
ReplaceOnce(json,
",\n \"accel_limit\": {\n"
" \"ax_mps2\": 2.0,\n"
" \"ay_mps2\": 2.0,\n"
" \"az_mps2\": 1.5\n"
" }",
"");
const std::string path = WriteTempJson(json, "vtc_cfg_nosection.json");
try {
TrackingConfig::LoadFromJson(path);
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("accel_limit"), std::string::npos);
}
std::remove(path.c_str());
}
TEST(VtcConfigTest, InvalidHeadingModeThrows) {
std::string json = kFullJson;
ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"magic\"");
const std::string path = WriteTempJson(json, "vtc_cfg_badmode.json");
try {
TrackingConfig::LoadFromJson(path);
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("heading.mode"), std::string::npos);
}
std::remove(path.c_str());
}
TEST(VtcConfigTest, InvalidNoDistanceActionThrows) {
std::string json = kFullJson;
ReplaceOnce(json, "\"no_distance_action\": \"hold\"",
"\"no_distance_action\": \"zoom\"");
const std::string path = WriteTempJson(json, "vtc_cfg_badaction.json");
try {
TrackingConfig::LoadFromJson(path);
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("no_distance_action"),
std::string::npos);
}
std::remove(path.c_str());
}
TEST(VtcConfigTest, NegativeFrequencyThrows) {
std::string json = kFullJson;
ReplaceOnce(json, "\"frequency_hz\": 20.0", "\"frequency_hz\": -1.0");
const std::string path = WriteTempJson(json, "vtc_cfg_negfreq.json");
try {
TrackingConfig::LoadFromJson(path);
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("control.frequency_hz"),
std::string::npos);
}
std::remove(path.c_str());
}
TEST(VtcConfigTest, FovOutOfRangeThrows) {
std::string json = kFullJson;
ReplaceOnce(json, "\"fov_h_deg\": 60.0", "\"fov_h_deg\": 200.0");
const std::string path = WriteTempJson(json, "vtc_cfg_badfov.json");
try {
TrackingConfig::LoadFromJson(path);
FAIL() << "应当抛出异常";
} catch (const std::runtime_error& e) {
EXPECT_NE(std::string(e.what()).find("camera.fov_h_deg"), std::string::npos);
}
std::remove(path.c_str());
}
} // namespace
reference-c++/visual_track_control/视觉跟踪控制律实现文档.md
+443
View File
@@ -0,0 +1,443 @@
# 视觉跟踪控制律实现文档（visual_track_control）
> 独立静态库子工程，实现 `../docs/物理追踪思路.md`（下称"规格"）定义的
> 视觉跟踪阶段（S7 VISUAL_TRACKING）控制律：水平航向、垂直速度、
> 距离 PID 三路控制 + 统一限幅 + Coast 清积分 / 判丢刹车 + 超龄降级，
> 输出端随机体航向完成机体系→局部 NED 旋转，经 `MavlinkCommandGateway`
> 打包为 MAVLink 字节流，交由注入的 `ICommandTransport` 实现送达。
>
> 本文档按 `../AGENTS.md`"实现文档规范"六要素组织。三份文档分工明确、
> 互不重复：
>
> - `../docs/物理追踪思路.md`——控制律**物理与行为规格**（唯一权威），
> 本文档不重复抄录公式推导；
> - `../docs/视觉跟踪控制律技术路线.md`——**语言无关实现路线**：单周期八步
> 伪代码、构件与状态清单、坐标系与符号约定、打包掩码取舍、重写指引；
> - 本文档——**C++ 落地说明**：公共头一览、构建与测试方式、日志行为、
> 排查与修改要点。
>
> 追踪库行为约定见 `../target_tracker/target_tracker.md`，同样只引用不复述。
---
## 一、功能职责与边界
命名空间 `drone::vtc`，头文件保护一律 `#pragma once`。
**做什么**：
- **三路控制律**（`TrackingControlLaw::Update()` 单周期计算）：
- 水平航向（规格 §4）：像素偏移 → 水平视线角 β → 增益 → 变化率限幅 →
角速度限幅，按 `HeadingMode` 配置输出速率式 `yaw_rate_dps`（默认）
或位置式 `yaw_deg`（绝对 NED 航向，见 §3.8）；
- 垂直速度（规格 §5）：垂直视线角 α → 统一速度支路 → `vz_mps`
垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
- 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
`vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级
（hold / slow_approach / exit）。
- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
`SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
全部在机体系完成。
- **坐标系旋转**（规格 §2.3）：输出端用 `RotateBodyToNed()` 把机体系
速度（前/右/下）随快照航向 ψ 旋转为局部 NED（北/东/地），与网关的
`MAV_FRAME_LOCAL_NED` 一致；仅绕下轴旋转（滚转/俯仰对速度指令为
二阶小量，由飞控内环处理）。
- **Coast 清积分**（规格 §6.4）：`is_predicted == true` 时清空距离 PID
积分项（`ResetIntegral()`），防积分饱和。
- **判丢刹车**（规格 §6.4）：`tracked == false` 时输出 `kBrakeHover`
零速刹车，并清积分与全部限幅器历史（保证恢复跟踪后指令从零渐变）。
- **超龄降级**（规格 §7.1、§9.2）：姿态过期/缺失 → `kDegradedHold`
（`kAttitudeStale`，禁止控制）；视觉结果超龄 → `kDegradedHold`
（`kVisionStale`）；判定器为无状态 `StalenessChecker::IsStale()`。
- **MAVLink 打包**（规格 §2.2）：`MavlinkCommandGateway` 把
`ControlOutput` 打包为 `SET_POSITION_TARGET_LOCAL_NED`（ID 84）字节流。
- **配置化**（规格 §8、需求分析 NFR-007）：全部阈值集中在
`TrackingConfig`，由 `TrackingConfig::LoadFromJson()` 启动期一次性加载，
代码不写死任何参数。
**不做什么**（边界）：
- **不做感知**：不运行检测/光流/雷达驱动，只消费调用方填入的
`ControlSnapshot`；
- **不做状态机仲裁**：S6/S8 切换、目标丢失策略（`ITargetLostStrategy`）、
障碍接管均在状态机层，本库只按输入标志产生当拍输出；
- **不写串口/传输**：库内无任何链路代码，字节流交给 `ICommandTransport`
实现方；
- **不解析 MAVLink 输入**：只有打包发送方向，无 `mavlink_parse_*`；
- **不做机体补偿**：`ControlSnapshot` 的 `roll_rad` / `pitch_rad` /
`yaw_rate_radps` / `velocity_*` 字段为预留输入（规格 §4.1）；当前仅
`yaw_rad` 参与计算（NED 旋转与位置式航向合成）；
- **不做热更新**：配置启动期加载一次，调参重启生效。
---
## 二、接口与数据流
### 2.1 公共头一览
| 头文件 | 内容 |
|--------|------|
| `include/visual_track_control/vtc_types.h` | `ControlMode`、`DegradedReason`、`ControlSnapshot`、`ControlOutput` |
| `include/visual_track_control/vtc_config.h` | `HeadingMode`、`NoDistanceAction`、各分组配置结构、`TrackingConfig` |
| `include/visual_track_control/tracking_control_law.h` | `TrackingControlLaw`（主控律） |
| `include/visual_track_control/pid_controller.h` | `PidParams`、`PidController` |
| `include/visual_track_control/rate_limiter.h` | `Clamp()`、`SlewRateLimiter`（header-only） |
| `include/visual_track_control/staleness_checker.h` | `StalenessChecker`（header-only） |
| `include/visual_track_control/line_of_sight.h` | `PixelToAngle`（像素→视线角，全程针孔模型） |
| `include/visual_track_control/frame_transform.h` | `RotateBodyToNed()`（机体系→局部 NED，header-only） |
| `include/visual_track_control/i_command_transport.h` | `ICommandTransport`（传输抽象） |
| `include/visual_track_control/mavlink_command_gateway.h` | `MavlinkCommandGateway`（MAVLink 打包） |
### 2.2 关键字段（以代码为准）
`enum class ControlMode`：`kVelocityHeading`（速度航向主输出）/
`kBrakeHover`（零速刹车）/ `kDegradedHold`（降级保持，禁止正常控制）。
（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。
`ControlSnapshot`（输入，纯值类型）：
| 字段 | 含义 |
|------|------|
| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
| `radar_distance_m` | 机头前向单点距离（米） |
| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） |
| `radar_time_ms` | 雷达取样单调时钟 |
| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
| `velocity_x_mps` / `velocity_y_mps` / `velocity_z_mps` | 自机速度分量（预留，接入主工程时由 `FlightStateSnapshot` 填充） |
`ControlOutput`（输出，已限幅）：
| 字段 | 含义 |
|------|------|
| `mode` / `valid` | 输出模式 / 指令是否可用于发送 |
| `vx_mps` / `vy_mps` / `vz_mps` | **局部 NED 速度分量（北/东/地，米/秒）**：各通道在机体系计算并限幅后，输出端随 `snapshot.yaw_rad` 旋转得到；旋转为正交变换，合速度大小不变。机体系侧向恒为 0，旋转后 `vy_mps` 可非零 |
| `yaw_deg` | 位置式目标**绝对航向**（度，NED，右转为正）：由当前航向 + 变化率限幅后的修正量合成，回绕到 [-180, 180)（ID 84 的 yaw 字段为绝对值） |
| `yaw_rate_dps` | 速率式偏航角速度指令（度/秒，右转为正；天然是相对量，无坐标系歧义） |
| `degraded_reason` | 降级原因（仅 `kDegradedHold` 有意义） |
| `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
`TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
`image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、
`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
`approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、
`no_distance_action`、`no_distance_approach_limit_mps`）、`accel_limit`
（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
### 2.3 ICommandTransport 契约
```cpp
class ICommandTransport {
public:
virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
};
```
- **只吃字节流**：串口、电台、回环打印均可注入实现；后续重写传输层
只换实现，不动打包与控制律；
- 返回 `true` = 已交付底层链路，`false` = 发送失败（调用方可计数/告警）；
- 实现方保证不长期持有 `data` 指针（同步拷贝或同步写出）。
### 2.4 数据流向
```text
推理侧每帧: TargetTracker::Update(detections, frame_shape)
控制线程(固定周期, 规格 §7.1):
TargetTracker::LastResult() # 线程安全拷贝
→ 取样瞬间用单调时钟打戳填入 ControlSnapshot.track_time_ms
→ TrackingControlLaw::Update(snapshot, now_ms) # 单周期计算
→ 按 ControlOutput.mode 分发:
kVelocityHeading → MavlinkCommandGateway::SendVelocityHeading()
kBrakeHover → MavlinkCommandGateway::SendBrakeHover()
kDegradedHold → 不发送（仅告警/降级）
→ mavlink_msg_to_send_buffer → ICommandTransport::Send(字节流)
停止视觉闭环: TrackingControlLaw::Reset()（规格 §6.4）
```
---
## 三、关键实现点
### 3.1 传输接口只吃字节流的双重解耦
打包（`MavlinkCommandGateway`）与传输（`ICommandTransport`）分离：
网关只负责 `ControlOutput` → MAVLink 字节，不感知链路形态；传输实现
只感知字节，不感知消息语义。对应规格 §2.2 控制单出口约束在独立库中的
映射——控制律不直接写飞控。
### 3.2 TrackResult 无时间戳，由调用方取样打戳
`TrackResult` 是追踪库自有类型，本身不含时间戳（规格 §7.2）。约定：
调用方在读取 `LastResult()` 的**瞬间**用单调时钟打戳填入
`track_time_ms`；超龄判定一律用"取样时间 + `valid_for_ms`"
（`StalenessChecker::IsStale(sample_time_ms, now_ms, valid_for_ms)`）：
- `sample_time_ms <= 0` 视为从未取样，直接判超龄；
- `now_ms < sample_time_ms`（时间戳不一致：时钟源错配/打戳错误）同样
判超龄——单一单调时钟下不应发生，宁可降级也不信任不可核实的取样。
### 3.3 PID anti-windup 策略（条件积分）
`PidController::Update()`（位置式）三层防护：
1. **积分钳位**：候选积分值先钳制在 ±`integral_limit`；
2. **anti-windup（条件积分）**：输出饱和（`|raw| > output_limit`）且
**本拍积分增量仍在推深饱和**（增量 `error·dt` 与原始输出 `raw`
同号）时，冻结本步积分累加（不采纳候选积分值）；误差反号后增量反向，
积分立即恢复回卷，无饱和残留。判据刻意基于"增量方向"而非"积分项与
输出同向"：后者在误差反号后仍会冻结回卷；也不用 `raw·error` 的
变体含混表述，实现即 `(error*dt_s) * raw > 0`；
3. **输出限幅**：最终输出钳制在 ±`output_limit`。
微分带一阶低通（`derivative_filter_coef ∈ [0,1)`，0 = 不滤波）；
`dt_s <= 0` 时保持上次输出（异常节拍保护）。`ResetIntegral()` 只清积分，
保留微分历史与上次输出；`Reset()` 清全部，均 O(1)（Coast 清积分，
规格 §6.4）。
### 3.4 SlewRateLimiter 兼作首条指令突变抑制
`SlewRateLimiter` 保存上次输出，单步变化不超过 `rate_limit · dt_s`；
构造时初值置 0，天然实现规格要求的"首条视觉速度设定值必须经过限幅"
（状态机设计 S6）：切换进入视觉跟踪后首条指令从零按加速度限幅渐变，
无需专门的切换分支。判丢进刹车时调用 `Reset()` 归零，恢复跟踪后同样
从零渐变。主控律共 4 个限幅器：`yaw_deg_limiter_` / `yaw_rate_limiter_`
（度/秒²）、`vx_limiter_` / `vz_limiter_`（米/秒²）。
### 3.5 像素→角度：全程针孔模型
`PixelToAngle` 构造期预计算针孔系数 `2·tan(θ/2)/size`，热路径仅一次
乘加 + `atan`：`angle = atan(offset·2·tan(θ/2)/size)`（规格 §4.1/§5.1）。
原"小偏移线性近似/大偏移针孔"双路切换已取消：线性式与针孔式在切换边界
两侧不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入
突变；`atan` 代价可忽略，全程采用精确针孔式。配置中 `linear_ratio` 键
随之移除。水平 β（`PixelToAngle(W, θ_h)`）与垂直 α（`PixelToAngle(H, θ_v)`）
各一个实例，构造期校验参数合法性（尺寸、视场角），非法抛
`std::invalid_argument`。
### 3.6 全工程仅 mavlink_command_gateway.cpp 一个 TU 包含 MAVLink 头
c_library_v2 的 `mavlink_helpers.h` 中 **`MAVLINK_HELPER` 默认为空宏**，
辅助函数直接以外部链接定义在头文件中；多个翻译单元包含 `common/mavlink.h`
会产生**多重定义**链接错误。因此：
- `common/mavlink.h` 只被 `src/mavlink_command_gateway.cpp` 包含
（该文件也是全库唯一的 MAVLink 知识落点）；单元测试同样**不得**包含
该头，按 MAVLink v2 线格式手工解码字节流断言（见 §5.3）；
- 公共头 `mavlink_command_gateway.h` 不出现任何 `mavlink_*` 类型，
打包/发送缓冲用成员固定字节存储承载（`msg_storage_` / `tx_buffer_`，
各 512 字节，`.cpp` 中 `static_assert` 校验 ≥ `sizeof(mavlink_message_t)`
与 `MAVLINK_MAX_PACKET_LEN`）；
- 打包用 `mavlink_msg_*_pack`（无通道状态、纯静态内联、可重入），
不使用 `_pack_chan` 通道 API；
- CMake 中 MAVLink include 目录设为 `SYSTEM PRIVATE`：压第三方告警噪音，
且不泄漏给下游。
同理，`nlohmann/json` 只被 `src/vtc_config.cpp` 一个 TU 包含。
### 3.7 配置启动期一次加载，不做热更新
`TrackingConfig::LoadFromJson()` 仅在启动期调用一次：解析结果拷贝进
`TrackingConfig` 值对象后即与 JSON 脱钩，热路径绝不触碰 JSON 对象；
调参重启即可。若未来需要热更新，扩展点为**原子指针交换**：持有
`std::atomic<const TrackingConfig*>`，后台解析新配置成功后原子换指针，
控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。
### 3.8 主控律单周期流程
`Update()` 按固定流程执行（`last_now_ms_` 推算 dt，首拍与异常节拍
（时钟回拨/`now_ms` 未前进）用 `1.0 / control.frequency_hz` 兜底，
dt 上限 `kMaxDtS = 0.5` 秒）：
① 超龄判定（姿态过期/缺失 → `kDegradedHold`；视觉超龄 → `kDegradedHold`）
→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
两个设计决策记录：
- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
→ ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息类型
逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路：小误差经
`vz_gain` 线性映射为小垂直速度，行为连续、全周期单一 offboard 消息。
- **位置式航向输出绝对值（A2）**：ID 84 的 `yaw` 字段是**绝对** NED
航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
（默认）输出角速度指令，天然相对量、无此问题。
### 3.9 MAVLink 消息映射（规格 §2.2）
全库仅 ID 84 `SET_POSITION_TARGET_LOCAL_NED` 一种消息（原 `kAttitudeTrim`
→ ID 82 `SET_ATTITUDE_TARGET` 已随姿态微调支路移除）：
| 模式 | 消息 | 要点 |
|------|------|------|
| `kVelocityHeading` | ID 84 | `coordinate_frame = MAV_FRAME_LOCAL_NED`（速度分量已是 NED 语义）。速度模式 `type_mask` 基础位 `kMaskVelocityBase = 0x01C7`（忽略位置与加速度），航向三分支：角速率非零 → `kMaskVelocityYawRate`（再忽略 yaw）；仅航向角非零 → `kMaskVelocityYaw`（再忽略 yaw_rate）；**两者均零 → 速率掩码配零角速率（保持当前航向）**——旧实现把零角速率落到航向掩码，等于误发绝对航向 0（指向正北） |
| `kBrakeHover` | ID 84 | `kMaskBrake = 0x0DC7`：速度全零，位置/加速度/航向/角速率字段全部忽略 |
---
## 四、日志行为
遵循 `../AGENTS.md` 日志纪律：**库内仅状态切换点打日志，热路径零日志、
零堆分配**。
| 位置 | 日志 | 触发条件 |
|------|------|----------|
| `tracking_control_law.cpp`（`Update()` 内 `FinishAndLog`） | `spdlog::info` "[ControlLaw] 状态切换: X -> Y (原因)" | **仅当 `ControlMode` 相对 `last_mode_` 发生变化**时打印（刹车进出、降级切换、进入/退出 Coast 跟踪）；同一模式连续多拍不重复打印 |
| `tracking_control_law.cpp` | —— | 每拍正常计算（含三路通道数学）**不打任何日志** |
| `mavlink_command_gateway.cpp` / `pid_controller.cpp` / `line_of_sight.cpp` / `vtc_config.cpp` | —— | 不打日志（配置错误走异常，见第六章） |
示例程序（`examples/track_control_demo.cpp`，非生产代码）另有两处
**演示观测日志**，均带节流防刷屏：
- `LoggingTransport::Send()`：字节统计与包头十六进制摘要，**秒级节流**
（距上次打印 ≥ 1000 ms 才输出）；
- 主循环状态摘要：**每 10 拍一次**（20 Hz 下即半秒一次）。
---
## 五、构建、测试与运行方式
### 5.1 构建
`../AGENTS.md` 规定构建环境为 WSL2 Ubuntu 24.04。**当前开发机 WSL 仅
装有 Ubuntu 22.04（发行版名 `Ubuntu`）**，本库的构建与测试验证在
22.04（cmake 3.22 / g++ 11.4）完成，行为与 24.04 无差异；24.04 发行版
安装后应按其复核一遍（见 §6.4）。
```bash
cmake -S visual_track_control -B build_vtc -DBUILD_TESTING=OFF
cmake --build build_vtc -j$(nproc)
./build_vtc/track_control_demo
```
- **为什么 `-DBUILD_TESTING=OFF`**：本库以 `add_subdirectory` 零改动复用
`target_tracker`，而 `target_tracker/CMakeLists.txt` 中 `include(CTest)`
会把 `BUILD_TESTING` 默认置为 `ON`，从而连带构建追踪库的 3 个测试
可执行文件。本子工程不关心其测试，显式关闭以加快构建（追踪库自身的
测试方式见 `../target_tracker/target_tracker.md` §五）。
- **本库自身测试不复用 `BUILD_TESTING`**：由独立选项 `VTC_BUILD_TESTS`
（默认 `ON`）控制，与上面的关闭互不干扰。
- **示例无需 `LD_LIBRARY_PATH`**：`target_tracker` 为 SHARED 库，
构建期 `CMAKE_BUILD_RPATH` 已指向构建目录，`track_control_demo`
可直接运行。
- 示例缺省配置路径由编译期宏 `VTC_DEMO_DEFAULT_CONFIG` 内置为
`config/track_control.json` 的绝对路径；也可命令行传入
`track_control_demo [配置文件路径]`。
### 5.2 示例演示内容
单线程模拟完整链路（合成运动检测框 → `TargetTracker::Update()` →
`LastResult()` + 打戳 → `TrackingControlLaw::Update()` → Gateway →
`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
（对应规格 §6.4）：
1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达
关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
角速度随 β 收敛而减小）；
2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold：
前向清零）；
3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
零速刹车。
示例将追踪库判丢阈值放大为 `kDemoMaxLostFrames = 60` 帧（默认 8 帧在
20 Hz 下 Coast 仅 0.4 秒不便观察），**仅演示专用**，正式参数以追踪库
规格为准。程序跑完总拍数后自行调用 `TrackingControlLaw::Reset()` 并
正常退出（返回 0），无需手动终止。
### 5.3 单元测试（Google Test）
测试源文件位于 `tests/`，覆盖 8 个测试套件：
| 测试文件 | 覆盖要点 |
|----------|----------|
| `rate_limiter_test.cpp` | `Clamp` 边界；`SlewRateLimiter` 单步限幅、`dt<=0`/冻结保护、`Reset` |
| `staleness_checker_test.cpp` | 从未取样、时钟回拨（`now < sample`）、恰在有效期边界（临界判新鲜） |
| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
运行方式（构建后）：
```bash
./build_vtc/vtc_unit_tests # 直接跑
ctest --test-dir build_vtc --output-on-failure # 或经 ctest（gtest_discover_tests 注册）
```
依赖：系统安装 Google Test（`libgtest-dev`，FindGTest 模块模式，无需
CMake config 包）；未找到时自动跳过测试构建并打印状态，不影响库与示例。
`vtc_config_test` 通过编译期宏 `VTC_TEST_CONFIG` 读取真实配置文件
`config/track_control.json`——**修改该配置文件时须同步核对测试断言**。
### 5.4 部署提示
`target_tracker` 为 SHARED 库（`libtarget_tracker.so.1`）：部署
`track_control_demo` 或未来接入主工程时，必须将该 so 一并随带
（目录式部署与 RPATH 约定见 `../target_tracker/target_tracker.md` §7）。
本库 `visual_track_control` 自身为 STATIC 库，链接进可执行文件，无需随带。
---
## 六、排查 / 修改要点
### 6.1 配置缺字段 / 非法值报错定位
`LoadFromJson()` 缺字段、类型错误、非法取值均抛 `std::runtime_error`，
**异常消息携带完整字段名**（如 `配置缺失字段或类型错误: heading.gain`、
`配置非法(必须为正): control.frequency_hz`、
`配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit`），
按异常文本直接定位到 `config/track_control.json` 的对应分组与键即可；
文件打开/解析失败则携带文件路径。`PixelToAngle` 构造期参数非法（尺寸
非正、视场角不在 (0, π)）抛 `std::invalid_argument`，同样见消息文本。
### 6.2 MAVLink 消息待实测适配项
以下各项均以 **PX4 1.11.3 Offboard 实测为准**（规格 §10.1 待定项），
全部集中在 `src/mavlink_command_gateway.cpp`，修改时只动这一个文件：
| 适配项 | 位置 | 现状 |
|--------|------|------|
| `coordinate_frame` | `SendVelocityHeading()` / `SendBrakeHover()` 的 `mavlink_msg_set_position_target_local_ned_pack` 调用 | 固定取 `MAV_FRAME_LOCAL_NED`；速度分量已由主控律旋转为局部 NED 语义（§3.8 步骤⑧），语义是否匹配待实测确认 |
| 速度模式 `type_mask` | 匿名命名空间 `kMaskVelocityBase` / `kMaskVelocityYaw` / `kMaskVelocityYawRate` / `kMaskBrake` | 掩码位组合待实测确认 |
### 6.3 接入主工程时需补齐的事项
1. **机体补偿（可选）**：机体系→NED 旋转已在库内完成（仅绕下轴按
`yaw_rad`，规格 §2.3）；`roll_rad` / `pitch_rad` / `yaw_rate_radps` /
`velocity_*` 字段仍为预留，规格 §4.1 的机体补偿启用时由姿态源填充；
2. **`TrackedTarget` 主题接线**：`TrackResult` 尚未进入主工程
`include/common/types.h`；需先按 `../target_tracker/target_tracker.md`
§7 与规格 §7.2 正式定义 `TrackedTarget` 消息与 `kTrackedTarget` 主题，
由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
（`ICommandTransport` 注入），打包与控制律零改动；
4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
### 6.4 红线声明
- `config/track_control.json` 与 `TrackingConfig` 默认值中**所有数值均为
占位值**（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/
畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库
**仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞**。
- 构建环境差异：`../AGENTS.md` 规定 WSL2 Ubuntu 24.04，当前开发机仅
装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。