多定位源的自动综合

概述 / Overview

"多定位源的自动综合"是 Detour 把多种定位手段（激光 SLAM、地纹 SLAM、天花板 SLAM、二维码、IMU、轮编里程计、RTK、UWB）融合为单一 6-DoF 位姿的机制。核心是 TightCoupler —— 一个紧耦合的位姿图优化器，根据每个观测源的协方差自动加权。

"Auto multi-source positioning fusion" is Detour's mechanism for merging multiple localisation sources (laser SLAM, ground-texture SLAM, ceiling SLAM, QR, IMU, wheel odometry, RTK, UWB) into a single 6-DoF pose. The core is the TightCoupler — a tightly-coupled pose-graph optimiser that weights each source by its covariance.

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

为什么需要 / Why fuse

任何单一定位源都有盲点： Every single source has blind spots:

源 / Source	强 / Strength	弱 / Weakness
激光 SLAM	几何信息丰富的工业场景精度高 / High accuracy in geom-rich industrial sites	长走廊 / 镜面 / 重对称场景退化
地纹 SLAM	平整地面 mm 量级 / Sub-cm on smooth floors	湿水 / 油污地遮挡
天花板 SLAM	地面动态时仍稳 / Stable when floor is busy	高反射 / 低净高场景
二维码	已知二维码处亚厘米 / Sub-cm where tags present	视场无二维码时无输出
IMU + 轮编里程计	高频、低延迟 / High-rate, low-latency	累积漂移，无全局信息
RTK GNSS	室外厘米级 / Outdoor cm-level	室内不可用
UWB	室内 ~10 cm / Indoor ~10 cm	多径效应 + 基站布设成本

多源融合让强项互补、弱项互救：例如长走廊上激光退化、地纹接管；天眼区有遮挡时二维码补全。 Fusion lets strengths complement and weaknesses recover: e.g. laser drops in long corridors while ground-texture takes over; QR catches up where ceiling is occluded.

紧耦合 / Tight coupling

"紧耦合"是与"松耦合"对比的术语： "Tight" vs "loose" coupling:

松耦合 / Loose: 每个源独立产生位姿，最后用 EKF 加权平均位姿。简单但损失原始观测信息。
紧耦合 / Tight: 每个源的 原始观测（约束）进入同一个位姿图，统一优化。MDCS 使用紧耦合。

Loose: each source emits a pose; EKF averages them. Tight: each source's raw observation enters one pose graph; everything optimised jointly. MDCS uses tight coupling.

外部反馈接口 / External-feed API

非 Detour 内置的定位源（轮编里程计 / IMU / RTK / UWB / 第三方视觉）通过 `TightCoupler.PostExternalFeed()` 接入：

Non-built-in sources (wheel odom / IMU / RTK / UWB / third-party vision) enter Detour through `TightCoupler.PostExternalFeed()`:

TightCoupler.PostExternalFeed(new ExternalFeed
{
    name           = "wheel_imu_2",
    counter        = tick++,
    hasTranslation = true,
    hasRotation    = true,
    is2D           = true,        // 2D 平面运动 / 2D planar
    integrated     = true,        // 这是个 ''累积'' 量 / cumulative
    toRemap        = false,       // 不是绝对参考系 / not absolute frame
    startingTick   = baselineTick,// 累积起算的基准 tick / baseline
    x = odomX, y = odomY, th = odomTh
}, name: "WheelImu");

主要字段：

`hasTranslation / hasRotation` — 提供的自由度
`is2D / is3D` — 平面 vs 3D
`integrated` — 累积量（必须配合 `startingTick`）还是单帧量
`toRemap` — true 表示绝对参考系（如 RTK），融合时需要做全局重映射

自动加权 / Automatic weighting

TightCoupler 不需要手动设置每个源的权重；权重 = 协方差矩阵的逆。每个源必须诚实汇报自己的协方差：

TightCoupler doesn't require hand-tuned per-source weights. Weight = covariance inverse. Each source must report its covariance honestly:

激光 SLAM：Hessian 求逆给出
二维码：从 PnP 重投影残差估计
IMU / 里程计：从硬件规格 + 时间间隔估计
RTK：从 GNSS 接收机 reported DOP

错报协方差是 融合崩坏的最常见原因 —— 一个谎报"我超准"的源会统治融合结果。 Mis-reporting covariance is the single most common cause of fusion blow-ups — a source claiming to be much more accurate than reality will dominate.

故障处理 / Failure handling

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

调试 / Debugging

Detour UI 的"融合面板"显示： The Detour UI's "fusion panel" shows:

每个源的最新观测时刻 + 协方差
当前主导源
残差直方图
协方差膨胀 / 收缩历史

打开 单源调试模式可以 强制只用一个源跑定位（用于诊断单源故障）。

A "single-source debug mode" forces the fusion to use only one source — handy for diagnosing a misbehaving one.