TightCoupler外部反馈API

概述 / Overview

`TightCoupler` 是 Detour 的 多源位姿融合器。非 SLAM 来源的位姿（轮编里程计 / IMU / RTK / UWB / 第三方视觉等）通过 `PostExternalFeed` API 推入 TightCoupler，与激光 / 地纹 / 二维码等 SLAM 后端一起做紧耦合优化，输出统一的 6-DoF 位姿。

`TightCoupler` is Detour's multi-source pose fuser. Non-SLAM poses (wheel odometry / IMU / RTK / UWB / third-party vision) enter through the `PostExternalFeed` API. They join the SLAM backends (laser / ground-texture / QR) in a tightly-coupled optimisation that emits the unified 6-DoF pose.

实现：`D:\src\Detour\DetourCore\Algorithms\TightCoupler.ExternalCoupler.cs:14`。 Implementation: `D:\src\Detour\DetourCore\Algorithms\TightCoupler.ExternalCoupler.cs:14`.

整体设计见多定位源的自动综合。 For the design rationale see multi-source positioning fusion.

API 签名 / API signature

public static void PostExternalFeed(ExternalFeed obj, string name);

每次调用提交一帧的外部位姿观测。`name` 是 来源标识（如 `"WheelImu"` / `"RTK"`），用来在融合器内部跟踪不同来源的协方差、检查超时、重映射。

Each call submits one frame of an external pose observation. `name` identifies the source (e.g. `"WheelImu"`, `"RTK"`) so the fuser can track per-source covariance, detect silence, and remap as needed.

`ExternalFeed` 结构体 / Struct

public struct ExternalFeed
{
    public string name;
    public long   counter;
    public bool   hasTranslation;
    public bool   hasRotation;
    public bool   is3D;
    public bool   is2D;
    public bool   integrated;
    public bool   toRemap;
    public long   startingTick;
    public float  x, y, z, th, pitch, roll;
}

字段语义 / Field semantics:

字段 / Field	含义 / Meaning
`name`	来源名（同上）；融合器按此聚类
`counter`	单调递增；用来检测丢帧 / out-of-order
`hasTranslation`	该观测包含位置（x, y, [z]）/ includes translation
`hasRotation`	该观测包含朝向（th, pitch, roll）/ includes rotation
`is2D` / `is3D`	二选一；表示是平面运动还是 3D 6-DoF
`integrated`	true = 累积量（相对位姿）；false = 单帧绝对量 / cumulative vs single-frame
`toRemap`	true = 绝对参考系（如 RTK），融合时需要全局重映射 / absolute frame, needs global remap
`startingTick`	累积量的基准 tick；用来检测重置（如 IMU 重启）
`x, y, z, th, pitch, roll`	实际位姿（mm / rad）

典型来源配置 / Canonical source configurations

`TightCoupler.ExternalCoupler.cs:49-54` 中的注释列出了典型用法： The source comment lists canonical configurations:

来源 / Source	标志组合 / Flags
轮编 + IMU / Wheel + IMU	hasRotation \| is2D \| integrated \| startingTick`
仅 IMU / IMU only	(is2D 或 is3D) \| integrated`
RTK GNSS	toRemap`（绝对，需重映射）
UWB / 基站 / Beacon	is2D`（绝对二维位置）
第三方视觉 / 3rd-party vision	视具体输出而定

提交模板 / Submission template

轮编 + IMU 紧耦合：

using DetourCore.Algorithms;

TightCoupler.PostExternalFeed(new ExternalFeed
{
    name           = "wheel_imu",
    counter        = tick++,
    hasTranslation = true,
    hasRotation    = true,
    is2D           = true,
    integrated     = true,
    startingTick   = baselineTick,
    x  = odomX,
    y  = odomY,
    th = odomTh
}, name: "WheelImu");

RTK ：

TightCoupler.PostExternalFeed(new ExternalFeed
{
    name           = "rtk",
    counter        = gpsTick++,
    hasTranslation = true,
    is2D           = true,
    integrated     = false,
    toRemap        = true,
    x = rtkEastingMm,
    y = rtkNorthingMm
}, name: "RTK");

协方差与权重 / Covariance & weighting

TightCoupler 内部按 协方差矩阵的逆加权 —— 不需要插件作者设权重；插件 必须诚实地报协方差。错报协方差是融合崩坏的首要原因：一个谎报"我超准"的源会统治融合结果。

TightCoupler weights by the inverse of the covariance matrix — no manual weight tuning needed; sources must honestly report covariance. Mis-reporting is the #1 cause of fusion blowups.

来源：

SLAM 后端：Hessian 求逆
二维码 PnP：重投影残差
IMU / 里程计：硬件规格 + 时间间隔
RTK：GNSS DOP

异常路径 / Failure paths

源静默 / Silence: 超过 timeout（融合器配置） → 该源从下一帧位姿图中删除，其它源不受影响。
跳变 / Wild jump: TightCoupler 内置 卡方检验；> 5σ 的观测直接丢弃。
融合发散 / Divergence: 残差持续上升 → 触发 软重启（保留主导源，重置其它源的位姿先验）。

调试 / Debug

Detour UI 的"融合面板" 显示：

每个源的最新观测时刻 + 协方差
当前主导源
残差直方图

打开 单源调试模式可强制只用一个源（用于诊断单源故障）。 There's a "single-source debug mode" forcing the fusion to use only one source.

与雷达 Lidar 插件的区别 / vs lidar plugins

雷达数据不走此 API：

雷达插件 `output()` 把帧推到 DObject；
Detour 端 `Lidar2D.ReadLidar()` 订阅该 DObject，把 SLAM 观测加到内部位姿图。

只有 非传感器的位姿来源走 `PostExternalFeed`。

Lidar data does NOT go through this API. Lidar plugins `output()` to DObject; Detour subscribes via `Lidar2D.ReadLidar()`. Only non-sensor pose sources go through `PostExternalFeed`.