SeeingTag：从 PnP 定位到单应性映射

SeeingTag 是为智能车赛道设计的俯拍定位系统：固定 ARUCO 标签定义赛道坐标，车载标签提供车辆位置与朝向，结果通过 UDP 发送给 Unity。

它最初并不是现在这套结构。项目经历了从检测与通信验证、多标签 PnP，到单应性矩阵映射的连续改动。真正推动架构变化的，不是算法在纸面上的优劣，而是现场条件逐项击穿了原先的假设。

我在这个项目中具体做了什么

这篇记录中的视觉定位主链路、配置化、调试工具和后续吞吐优化，是我持续修改与验证的部分。Unity 接收端和整车系统属于更大的团队项目，SeeingTag 的职责是提供低延迟、坐标约定稳定的位置与偏航数据。

这样的边界很重要。定位程序“能够输出坐标”，不等于整套车辆控制已经由它完成；反过来，上位机出现延迟或朝向异常，也不一定来自车辆控制。项目早期保留 UDP 监听器、摄像头测试和独立定位模式，就是为了能沿系统边界排查问题。

第一版：先验证闭环

最早的任务只有两个：

• 摄像头能否稳定识别 DICT_4X4_50 标签；
• 位置数据能否以 JSON 通过 UDP 连续送达接收端。

这一阶段保留了独立的摄像头、发送端和监听端测试脚本，并在数据中加入 seq 与 timestamp。这样做看似简单，却让后续调试能够快速区分“视觉没有结果”和“网络没有送达”。

标签误检是第一个实际问题。解决方式并不是立即引入复杂分类器，而是先按标签在画面中的像素宽度过滤。对固定尺寸、固定拍摄高度的赛道，这个约束足够便宜，也足够有效。

这一阶段的验证标准也很朴素：

• 相同标签能否连续被识别，而不是偶尔出现；
• seq 是否单调增加，监听端能否发现丢包；
• 摄像头失败、未检测到标签和 UDP 发送失败能否被区分；
• 修改标签宽度阈值后，误检率和远处标签漏检如何变化。

原型的价值不是证明最终方案正确，而是把一个大问题拆成几段可单独证伪的链路。

早期定位原型。先确认标签识别、姿态估计与调试信息能够在同一条链路中持续输出。

PnP 方案：把相机和车辆放进世界坐标

第一套完整定位使用两次 PnP：

1. 合并多个固定标签的 3D 世界角点与 2D 图像角点，求相机位姿；
2. 对车载标签单独求相对位姿，再转换到世界坐标。

它能够输出车辆的三维位置与偏航角，算法链也很标准。但它依赖一个关键前提：相机内参和畸变模型必须足够可信。

当时没有完成正式标定，只能根据视场角估算内参。程序可以运行，绝对位置却会受到镜头参数误差影响。为方便现场调整，后来又加入了配置文件、校准模式、HUD、标签级低通滤波和独立的 UDP 发送封装。

回头看，这里有一个值得记录的误判：我最初把“算法能给出连续结果”近似当成“定位模型有效”。PnP 几乎总能返回一个数值，但数值存在并不代表它在几何上可信。缺少准确内参时，误差会被位姿求解吸收，画面看起来稳定，绝对坐标仍可能系统性偏移。

因此，后续调试不再只看是否输出，而是增加了赛道范围、固定点映射和朝向一致性检查。

现场暴露的三个问题

真正改变方案的是摄像头。

手机 RTSP 画面能够覆盖赛道，却出现 5 到 30 秒的累积延迟；普通 USB 摄像头延迟低，但视野只能覆盖一到两个固定标签；广角画面虽然覆盖更大，边缘桶形畸变又会破坏 PnP 对角点和内参的假设。

继续修补 PnP 意味着同时解决：

• 低延迟视频采集；
• 准确的相机内参与畸变标定；
• 足够大的赛道覆盖；
• 固定标签在画面中的稳定可见性。

赛道定位只关心地面上的二维位置和朝向，并不真正需要完整的相机三维位姿。这个需求差异，最终给出了重构方向。

早期全场调试画面。固定标签可见数为 3/4，车载标签尚未被识别；固定参考点覆盖不足，使单应性初始化无法稳定建立。

被放弃的路径为什么没有继续

当时并非只有“换成 Homography”一个选项。还可以正式标定广角镜头、对每帧先做去畸变，或者升级相机与采集硬件后继续使用 PnP。

这些方案在三维定位任务中可能更合理，但对当前赛道带来额外成本：

• 标定参数与具体镜头、分辨率和焦距绑定，现场更换设备后需要重做；
• 全帧去畸变增加处理开销，也不能解决 RTSP 累积旧帧；
• 高质量相机和采集卡能改善图像，却提高了部署复杂度；
• 系统最终只消费二维地面坐标，完整三维位姿没有被真正利用。

放弃 PnP 不是因为它“落后”，而是因为继续维护这些前提的收益不足以覆盖成本。

改用 Homography：只解决真正需要的问题

新方案使用固定标签角点建立图像平面到赛道平面的单应性矩阵，再将车载标签中心直接映射为世界坐标：

固定标签图像角点 + 已知赛道坐标
                ↓
        findHomography + RANSAC
                ↓
车载标签中心像素 → perspectiveTransform → 赛道坐标

朝向也不再从三维旋转矩阵中提取，而是把车载标签的两个方向角点映射到赛道平面，再计算方向向量。

这次改动删除了主链路对相机内参的依赖，也让问题更容易观察：映射矩阵是否成功建立，输出点是否位于合理赛道范围，都能直接显示在俯视图上。

代价同样明确。所有参考点必须近似共面，首次初始化需要四个固定标签可见，而且系统不再输出有意义的高度与俯仰信息。对当前赛道任务而言，这些代价可以接受。

更换广角相机后的鸟瞰视图。四个固定标签同时可见，车辆位置被映射到赛道平面。

如何判断重构真的有效

重构后的判断不依赖单一 FPS 数字，而是分成四层：

层次	检查内容	失败时优先怀疑
采集	实际相机帧率、是否持续取得新帧	摄像头、编码格式、缓冲
检测	固定标签与车载标签可见率	光照、尺寸阈值、运动模糊
几何	固定点重投影、赛道边界、朝向方向	标签坐标、角点顺序、映射矩阵
输出	UDP 频率、数据年龄、Unity 响应	队列、滤波、网络、坐标约定

这套拆分后来比选择哪一种算法更有价值。它让“画面很卡”“车有点飘”这类模糊描述能够落到具体链路。

稳定不是把滤波系数越调越小

定位抖动随后暴露出另一个取舍：平滑和延迟互相牵制。

系统先对检测结果做标签级滤波，再对最终位置和朝向做输出级滤波。双层结构让噪声来源更容易区分，但现场又发现，过小的输出系数会让 Unity 中的车身朝向明显滞后。

因此调参目标从“画面看起来最稳”改成“在可接受抖动下尽量降低控制与显示延迟”。后续加入持久化的车辆朝向偏置，也是同一思路：把标签安装角度造成的系统误差交给明确配置，而不是混进滤波或坐标代码里。

这里的失败经验是：静态画面最平滑的参数，通常不是动态跟踪最好的参数。滤波器必须用车辆转弯、加减速和短时遮挡来测试。只让车静止在赛道中央观察抖动，会系统性地把参数调得过慢。

后续改进：吞吐、失锁与可观察性

当定位正确后，瓶颈转向了时效性。采集线程改为持续读取、处理线程只取最新帧，避免旧帧在队列中排队；显示刷新与定位发送解耦，性能统计则分别记录相机、处理和发送频率。

短时丢失车载标签时，系统不会把任意历史状态无限外推。盲区策略需要手动武装，并同时受时间、距离和偏航变化限制。界面会显示 OFF / ARMED / DRIVING，让运行状态不只藏在日志里。

这段迭代最重要的经验并不是“Homography 比 PnP 好”，而是算法必须与任务需要和现场条件匹配。PnP 解决的是更完整的三维位姿问题；单应性映射放弃了不需要的信息，换来了更短的依赖链和更直接的调试方式。

相机帧率上限：请求 60 fps 不等于真的得到 60 fps

在定位程序的相机探测中，我分别请求了 1920×1080、1280×720、640×480 三种分辨率下的 30 fps 与 60 fps。实际读取频率始终约为 29.8～29.9 fps；所有 60 fps 请求都被标记为 LOW_FPS。这说明当前相机源在这台机器上的有效输出被锁在约 30 fps，并不是把分辨率调低或把目标帧率改成 60 就能突破。

这个结果也划清了性能边界：即使后续处理更快，定位状态也无法可靠地以高于约 30 Hz 的频率获得新的视觉输入。若虚拟赛道或显示端刷新更快，仍需通过显示侧插值、解耦刷新，或更换采集硬件来处理；不能把模型内部的高 FPS 误当成定位更新频率。

相机帧率探测输出。不同分辨率下请求 60 fps 时，实际读取频率仍约为 29.8～29.9 fps。

仍未解决的问题

当前方案仍有明确边界：

• 标签严重遮挡时，单帧视觉定位没有独立冗余；
• 映射精度依赖固定标签坐标和地面共面条件；
• 盲区外推只能处理很短的失锁，不能替代车载里程计；
• 现场还缺少系统化的轨迹真值，用来量化位置和偏航误差。

下一步若继续研究，我更希望加入轮速或 IMU 形成短时融合，并建立一条带真值采样点的测试轨迹。这样改进就能从“肉眼看起来更稳”走向可重复的误差比较。