# Detour 定位接口信息确认（源码答复）

用途：答复控制程序（`MultiWheelC` / `StateEstimation`）使用定位结果所必需的接口语义。
依据：本仓库 `DetourCore` 源码（`Location.cs`、`TightCoupler.cs`、`Frame.cs`、`LidarOdometry.cs`、`LidarMap.cs`、`WebAPI.cs`、`G.cs`）。
仓库内**没有**名为 `getCartLocation()` 的函数；控制侧该调用对应 Detour 的两类对外位姿出口。下文按字段对齐后统一说明。

---

## 对外位姿出口（先对齐接口）

控制程序看到的 `x / y / theta / tick / l_step`，来自下面之一（或对它们的封装）。字段名在源码里是 `th`，不是 `theta`。

| 出口 | 入口 | 字段 | `tick` 的真实来源 |
|---|---|---|---|
| HTTP `GET /getPos` | `CartLocation.FormatPosition()` | JSON：`x, y, th, l_step, tick`，异常时多 `error` | `tick = CartLocation.st_time`（观测时刻） |
| 共享对象 `DetourPos{shareObjectTag}` | `TightCoupler.CommitLocation` 每次提交后 `Post` | 二进制：`x, y, th, tick, l_step, error` | `tick = DateTime.Now.Ticks`（提交/推送时刻） |

默认 HTTP 端口：`Configuration.conf.guru.DetourPort`，默认 `4321`。
`shareObjectTag` 必须与控制侧 Clumsy 的 soTag 一致。

两种出口的 `x/y` 都已经过：

```
x_out = 内部x / guru.inputScale + guru.biasX
y_out = 内部y / guru.inputScale + guru.biasY
```

默认 `inputScale = 1`、`biasX = biasY = 0`，因此默认单位就是内部单位（毫米）。
`th` / `l_step` 不做缩放。

---

## 1. `getCartLocation()` 字段定义

### 1.1 各字段含义

| 字段 | 源码名 | 含义 |
|---|---|---|
| `x` | `CartLocation.x` | 车体原点在**当前地图坐标系**中的 X。已从传感器坐标系经安装外参反变换到车体中心（`TightCoupler.CommitLocation` 里 `ReverseTransform(传感器位姿, 组件安装位姿)`）。 |
| `y` | `CartLocation.y` | 同上，Y。 |
| `theta` | `CartLocation.th` | 车体航向。`0°` 朝向地图 `+X`，逆时针为正（内部用 `cos(th)` / `sin(th)` 画朝向）。 |
| `tick` | 见上表 | **不是**统一的一种时间。HTTP 是观测时间 `st_time`；DObject 是提交瞬间的 `DateTime.Now.Ticks`。控制侧必须先确认自己走的是哪条通道。 |
| `l_step` | `Frame.l_step` | **到已标注（labeled）关键帧的图上步数**，用来表达“离可信锚点有多远”，**不是**优化迭代次数，也不是匹配分数。注释原文：`steps to labeled keyframe`。 |

### 1.2 单位、正方向、坐标系

- **内部单位**：`x`、`y` 为毫米。UI 直接按 `mm` 显示。
- **对外单位**：默认仍是毫米；只有改了 `guru.inputScale`（例如设为 `1000` 输出米）才会变。
- **角度单位**：度。
- **坐标系**：当前加载的 SLAM 地图坐标系。原点由建图/标注关键帧决定，不是 GNSS 或车体启动点。
- **车体姿态**：输出的是**车体原点**，不是雷达原点。雷达/相机安装位姿在 `layout.components` 的 `x,y,th` 里。
- **右手平面**：`+X` 为航向 0，`+Y` 为航向 +90°。

### 1.3 `theta` 取值范围

**不是严格的 `[-180°, 180°]`。**

`LessMath.normalizeTh` 只在绝对值 ≥ 360 时按 360° 回绕，`(-360, 360)` 内原样返回。因此对外可能看到 `200`、`-270` 这类值。控制程序若假设 `[-180, 180]` 或 `[0, 360)`，应自己归一化。内部比较角度用 `LessMath.thDiff`，会按最短弧处理。

### 1.4 一次调用的字段是否同一帧

**是。** 一次读取对应同一个 `CartLocation` 对象上的 `x/y/th/l_step/st_time`。
`getPos` 先用当前 `latest` 判断 Timeout/Unstable，再序列化；若这两步之间恰好发生新提交，状态字和位姿可能差一帧，但单次 JSON 里的数值字段仍来自同一个对象。

---

## 2. `tick` 的准确含义（优先）

### 2.1 它是什么时刻

分通道：

**HTTP `/getPos` 的 `tick` = `st_time`**

- `st_time` 在 `Frame` 上的注释是：传感器数据**到达 Detour 时**由 `G.watch` 打的时间，不是激光头硬件曝光时刻，也不是 HTTP 返回时刻。
- 激光采集线程会按扫描周期做轻微平滑，并加上 `time_bias_ms`。
- 里程计把 `frame.st_time` 原样交给 `TightCoupler.CommitLocation`，再写入 `CartLocation.st_time`。
- 因此：`tick` ≈ **该位姿所对应的那帧观测到达时刻**。SLAM 计算发生在这之后，接口返回更晚。

**DObject `DetourPos` 的 `tick` = `DateTime.Now.Ticks`**

- 这是 **TightCoupler 提交并推送的本地时刻**，100 纳秒为单位，从公元 1 年 1 月 1 日起算（.NET `DateTime.Ticks`）。
- 它**不是**传感器采集时刻，也**不是** Unix epoch。
- 受本机本地时钟、时区、校时影响，必要时可能回跳。

### 2.2 单位、频率、单调、回绕

| 项目 | HTTP `tick`（`st_time`） | DObject `tick`（`DateTime.Now.Ticks`） |
|---|---|---|
| 单位 | 毫秒 | 100 ns（1 ms = 10000） |
| 时钟 | 进程启动时锚定一次 UTC，之后用 `Stopwatch` 单调累加：`ElapsedTicks*1000/Frequency + stMillis` | 本机本地 `DateTime.Now` |
| 更新频率 | 随传感器/融合提交，通常接近雷达帧率（常见 10–20 Hz，取决于设备 `timeBudget`） | 每次 `CommitLocation` 成功推一次 |
| 单调 | 进程内单调递增（不受事后改系统时间影响） | 一般递增；改系统时间或 DST 可能回跳 |
| 回绕 | `long` 毫秒，实际不会回绕 | `long` ticks，实际不会回绕 |
| 进程重启 | 重新锚定当前 UTC，**不会从 0 开始**，但与重启前不保证连续 | 继续跟本机时钟，与进程无关 |

`G.watch.TimeStampMillis` **不是** Unix 毫秒（1970），而是从公元 1 年起算的 UTC 毫秒，再加上启动后的单调流逝。

### 2.3 多车能否用 `tick` 对齐

**不能当作已同步的多车时钟。**

- 各车各自启动 `G.watch`，没有 PTP/NTP 协议，也没有跨车时间服务。
- HTTP `tick`：若各车系统 UTC 在启动时大致同步，数值会接近“同一套绝对时间”，但仍有启动锚定误差和各车处理延迟，**不能直接当多车位姿对齐的主时钟**。
- DObject `tick`：本地时间 ticks，跨时区会直接错开，更不适合多车对齐。
- 进程重启、暂停（`G.paused`）、提交失败都会造成时间空洞。

控制侧建议：单车内部用 `tick` 做延迟估计和短时预测；多车对齐应使用外部统一时钟，或只把 Detour `tick` 当相对时间。

---

## 3. 定位延迟与更新方式

### 3.1 输出位姿比真实运动滞后多少

源码**没有**标定“官方延迟 xx ms”。能确定的是延迟结构：

```
真实运动 → 雷达扫描/到达(st_time) → 里程计配准 → TightCoupler 融合 → 写入 CartLocation.latest → 接口读到缓存
```

经验上由源码阈值约束：

- 激光一圈通常几十毫秒（`Lidar2DStat.timeBudget`）。
- TightCoupler 常用窗口 `TCtimeWndSz = 150 ms`，上限 `TCtimeWndLimit = 700 ms`。
- `LidarOdometry` 把 `reg_ms > 200` 记为 `bad perf`。
- `/getPos` 若 `now - st_time > 500 ms` 打 `Timeout`。

因此正常运行时，**从观测到可读位姿大约是 1 帧雷达周期 + 配准/融合，常见几十到两百毫秒；超过 500 ms 会被 HTTP 接口判超时。** 这是实现上的门槛，不是出厂标定值。

### 3.2 `getCartLocation()` 是否返回最近一次缓存

**是。**

- `CartLocation.latest` 是全局缓存。
- 只有 `TightCoupler.CommitLocation` 成功后才会替换（取 `history` 中 `st_time` 最新的一帧）。
- `/getPos` 只读缓存，不触发新的 SLAM 计算。
- 配准失败、`CommitLocation` 返回 `null`（`bad variance` / `bad trace`）、或 `G.paused` 时，缓存停在上一帧。

### 3.3 原地自转时频率/延迟是否明显变化

**轮询频率不变，有效更新和延迟会变差。**

- 接口仍按调用方频率读同一缓存。
- 原地旋转时二维扫描重叠变差，帧间分、相位锁分容易掉，`allowCommit` 更常失败，缓存更容易停住。
- 配准变慢时单帧 `reg_ms` 上升；失败后 `l_step` 被加大（见第 4 节），看起来像“转的时候定位变钝”。
- 源码没有“自转专用降频”。

控制侧：自转短时预测应假设**有效位姿更新可能变稀、变老**，不要假设 `tick` 仍按雷达周期前进。

---

## 4. `l_step` 的含义（优先）

### 4.1 它是什么

**定位质量的图距离，不是优化迭代次数，也不是匹配状态枚举。**

定义：当前位姿/关键帧沿约束图走到**已标注关键帧**还要几步。

- `0`：本身就是标注帧（`labeledXY` / `labeledTh`），或刚被标成锚点。
- `1`：刚和地图/标注帧配准成功（`LidarMap` 成功后会把 `compared.l_step = 1`）。
- 正常递推：新关键帧 `l_step = 旧关键帧.l_step + lstepInc`。
- TightCoupler 提交时：有参考关键帧则 `reference.l_step + 1`；没有则 `latest.l_step + 3`。
- `9999`：未定位、手动设位、或与标注图断开。这是 `Frame` / 初始 `CartLocation` 的默认值。

**越小越好、越可信。**
`AIImplementationNote.md` 里“越小越不确定”与源码相反，不要采用。

内部用途包括：

- TightCoupler 边权：`l_step > 10 → 1`，`> 5 → 2`，`> 2 → 3`，否则 `4`；标注帧权 1000。
- 图优化用 `1/(l_step+0.01)` 当权。
- `l_step` 大时 `LidarMap` 更容易走全局配准（`forceGRegStep`、`GregThresK * l_step`）。
- `l_step > 1000` 且孤立的关键帧可被删掉。

源码 TODO 也写过：希望将来“去掉 `l_step`，改用误差圆”。当前对外接口仍是这个整数。

### 4.2 为什么原地自转会从 2～4 升到几十、上百

自转本身不会改公式，但会让 **`lstepInc` 连续被惩罚**，再在切关键帧时一次性加到 `l_step` 上。

`LidarOdometry` 里常见加项：

| 条件 | `lstepInc` 增量 |
|---|---|
| 帧间隔过大 | `+3`～`+20` |
| 时间落后 > 1 s | 可到 `+1000` 量级并重启局部图 |
| 有效点太少 | `+3` |
| 掩膜后点过少 | `+10` |
| 帧间序贯配准失败 | `+5` |
| 局部里程计分过低 | `+15` |
| 历史分偏低 | `+1`～`+15` |
| 局部图重启 / 相位锁差 | `+2` |
| 上一帧能提交、这一帧不能 | `+100` |
| 离开 hard 区域 | 默认再 `+20` |

原地自转时典型连锁是：旋转导致重叠变差 → 分数掉 → `lstepInc` 累加 → 因“新点变多 / 超时 / restart”切关键帧 → `新 l_step = 旧值 + lstepInc`。
若此时地图匹配也失败，TightCoupler 按 `latest.l_step + 3` 继续推高。
所以 2～4 很快可以变成几十，失败恢复前看到上百是符合实现的。

匹配一旦重新成功，关键帧会被改回 `l_step = 1`，输出也会掉下来。

### 4.3 有没有官方阈值

**没有写给控制程序的官方阈值表。** 下面是源码内部实际在用的分档，可作控制侧初值，现场仍要按地图和雷达标定。

| `l_step` | 源码含义 | 给控制程序的建议 |
|---|---|---|
| `0` | 标注锚点 | 最可信 |
| `1`～`2` | 刚贴上地图 / 离锚点很近 | **正常，接受** |
| `3`～`5` | TightCoupler 已降权 | 可用，开始警惕跳变 |
| `6`～`10` | 权更低；地图更倾向全局配准 | **退化，降低信任 / 收紧预测** |
| `11`～`98` | 最低权；曾从不稳定区回到 `2` 时会打工作区快照 | **明显退化，不宜做精细控制** |
| `≥ 99` | TightCoupler 允许关键帧被推得更狠 | 按不可靠处理 |
| `≥ 1000` | 孤立帧可删 | 基本断开 |
| `9999` | 未定位 / 手动设位 / 断开 | **不可用，应安全停止或等待重定位** |

Detour 自己判断“车是否静止可做全局重定位”时用：`l_step > 2` 且 TightCoupler 速度估计很小。这是内部策略，不是对外状态机。

---

## 5. 坐标跳变与重定位行为（优先）

### 5.1 重定位、回环、失败恢复后，`x/y/theta` 会不会永久跳变

**会。跳变是设计行为，不是毛刺。**

会改当前输出的情况：

1. **地图配准成功**：`LidarMap` 把当前关键帧改到匹配位姿，并 `l_step = 1`，再交给 TightCoupler。下一帧 `CartLocation` 跟着新参考走，表现为一次台阶。
2. **全局重定位**（`/relocalize` 或 UI Relocalize）：定位器进入 `relocalizing`，按全图关键帧搜索（`source = 9`）。第一次更好的匹配会 `TightCoupler.Reset`，位姿被拉到地图上，通常是大幅度永久跳变。
3. **回环 + `GraphOptimizer`**：关键帧 `x/y/th` 被就地改写。当前参考帧若被挪动，后续融合位姿跟着变。优化有动量平滑，但仍可能出现肉眼可见的台阶。
4. **手动 `/setLocation`**：直接改 `CartLocation.latest` 并重置融合窗口。
5. **匹配长期失败后突然恢复**：从里程计漂过的位置一下子贴回地图，跳变幅度等于累计漂移。

没有“只在内部跳、对外插值抹平”的保证。控制程序必须自己做跳变连续化或拒绝。

### 5.2 跳变后还在原来的地图坐标系吗

**同一张已加载地图内：是。**
跳的是车在这张图里的估计，不是换了一套轴。

会换坐标系的只有：换图（`/loadMap`）、`Remapper`（GNSS/外参映射）、或手动把车标到另一个锚点。这些不是普通重定位。

### 5.3 有没有“正在重定位 / 定位丢失 / 地图坐标调整”标志

**定位结果包里没有这些标志。**

内部有、但**不随 `getPos` / `DetourPos` 下发**：

| 内部量 | 作用 | 是否对外 |
|---|---|---|
| `Locator.relocalizing` / `relocalized` | 地图层正在/已经全局搜 | 否 |
| `G.IsSettingPosition` | 正在手动设位 | 否（`/getStat` 的 `globalStat` 里能看到） |
| `G.paused` | 定位暂停 | 否（同上，或调 `/pause` `/resume`） |
| `CartLocation.unstable` | 本帧掩膜过狠、场景不稳定 | **仅 HTTP**：`error = "Unstable"`。DObject 当前推送的 `error` 被写成空串 |
| `l_step` 升到很大 / `9999` | 实际的“丢了” | 是，但这是间接指标 |
| 图优化改关键帧 | “地图坐标调整” | 无单独标志 |

HTTP `/getPos` 仅有的显式错误：

- `"Timeout"`：`st_time` 已超过 500 ms
- `"Unstable"`：`latest.unstable == true`

没有 `"Relocalizing"`、`"Lost"`、`"MapAdjusted"`。
`/getStat` 可拉到各模块 `StatusMember`（雷达间隔、里程计状态、TC 状态等），但不是每帧位姿附属字段，也不适合当硬实时互锁。

控制侧应自己构造状态：

- **疑似丢失**：`error` 为 Timeout/Unstable，或 `l_step ≥ 99`，或 `tick` 长期不涨。
- **疑似重定位/回环跳变**：相邻两帧 `x/y/th` 突变，同时 `l_step` 突然掉回 1～2。
- **地图在拧**：跳变较缓、持续多帧，且 `l_step` 并不爆掉。

---

## 6. 可用的定位质量接口（优先）

### 6.1 除 `l_step` 外还能拿到什么

**位姿包几乎只有 `l_step` + HTTP 的 `error`。**

| 信息 | 有没有 | 说明 |
|---|---|---|
| 匹配得分 | 对控制程序：无 | 在 `LidarOdometry` / `LidarMap` 内部（`score`、`phaselocker_score`），不下发 |
| 协方差 | 对控制程序：无 | `Frame.errXX/errXY/errYY` 已预留，**未填进 `/getPos` 或 `DetourLocation`** |
| 置信度 | 间接 | 就是 `l_step`；源码 TODO 想换成误差圆，尚未做 |
| 定位状态 | 很弱 | HTTP：`Timeout` / `Unstable`；DObject：`error` 目前恒为空 |
| 错误码 | 无枚举 | 只有上述字符串 |
| 系统状态 | 有，但是慢接口 | `GET /getStat` → 布局/里程计/定位器/TC/GO/`G.paused`/`G.IsSettingPosition` |

因此控制程序**不能**指望每帧拿到匹配分或协方差。

### 6.2 Detour 实际用什么条件接受一帧（可当作官方内部规则）

源码里“接受并提交”的条件是分层的，没有单独的对外规范文档：

1. **里程计层**（`LidarOdometry.updateLocation`）
   - 分数过低会把 `strength` 压下去，甚至不把 `reference` 挂上。
   - 掩膜过狠 → `unstable = true`。
   - 点太少、序贯/局部配准失败 → 不切健康关键帧，并加大 `lstepInc`。
2. **融合层**（`TightCoupler.CommitLocation`）
   - 传感器处于 `bad variance` / `bad trace` → 直接丢弃，返回 `null`。
   - 丢弃后该源会被禁一段时间（约 1～2 s）。
3. **地图层**（`LidarMap`）
   - `result.score < ScoreThres` 丢弃。
   - 相对已有位姿的 `xy/th` 偏差超过按 `l_step` 放大的门限则丢弃（防止乱跳）。
   - 落到无效区域丢弃。
4. **HTTP 出口**
   - 缓存超过 500 ms → `Timeout`。
   - `unstable` → `Unstable`。

### 6.3 建议控制程序如何接受/拒绝一帧

源码没有写给 `MultiWheelC` 的官方判据。按上面的内部规则，建议：

**接受（正常闭环）**

- HTTP：无 `error`（DObject：至少 `tick` 在前进）。
- `l_step ≤ 5`。
- `tick` 新鲜（HTTP：`now - tick < 300 ms` 较稳妥；DObject：换算成 ms 后同样看提交间隔）。
- 相对上一接受帧：位移/转角不超过本底盘短周期能达到的上限。

**降级（短时预测、降低增益、禁止精细对位）**

- `6 ≤ l_step ≤ 10`，或偶发 Timeout 后立刻恢复。
- 原地自转期间 `l_step` 爬升但 `tick` 仍在更新。

**拒绝并安全停止 / 等待**

- `error == "Timeout"` 持续，或 `error == "Unstable"`。
- `l_step ≥ 99`（含 `9999`）。
- 单帧出现与运动学不符的永久台阶（尤其伴随 `l_step` 从很大突然回到 1）：先当重定位跳变，做连续化或刹停，不要直接当编码器。

**不要做的事**

- 不要用 `l_step` 当 ICP 迭代次数或“正在计算中”。
- 不要假设 `theta ∈ [-180, 180]`。
- 不要用 `tick` 做多车时间同步主时钟。
- 不要假设 DObject 的 `error` 会带 Timeout/Unstable（当前实现是空的）。

---

## 对 `StateEstimation` 的直接含义

清单里最优先的 2 / 4 / 5 / 6，对应控制侧应这样定：

1. **时间同步**
   - 先确认 `getCartLocation` 走 HTTP 还是 `DetourPos`。两条通道的 `tick` **单位和语义都不同**。
   - 单车：用 `tick` 估延迟、做自转短时预测。
   - 多车：另选同步时钟。

2. **自转短时预测**
   - 自转时有效更新可能变稀，`l_step` 会从个位数爬到几十上百。
   - 这是质量变差，不是接口卡死。预测窗口应随 `tick` 变老、`l_step` 变大而缩短。

3. **跳变连续化**
   - 重定位、回环、失败恢复都会造成**同地图下的永久台阶**。
   - 没有“正在改图”标志；用位姿差分 + `l_step` 回落来识别。

4. **安全停止**
   - 硬条件：`Timeout` / `Unstable` / `l_step ≥ 99` / `tick` 停更。
   - `l_step` 建议按 `≤5` 正常、`6–10` 退化、`>10` 不可用于精细控制。

---

## 源码锚点

| 主题 | 位置 |
|---|---|
| 对外 JSON / Timeout / Unstable | `DetourCore/Location.cs`：`ConstructRet`、`FormatPosition` |
| HTTP 路由 | `DetourCore/WebAPI.cs`：`/getPos`、`/setLocation`、`/relocalize`、`/getStat` |
| DObject 推送与 `tick=DateTime.Now.Ticks` | `DetourCore/Algorithms/TightCoupler.cs`：`DetourLocation`、`CommitLocation` |
| `l_step` 定义 | `DetourCore/Types/Frame.cs` |
| 车体中心变换、提交缓存 | `TightCoupler.CommitLocation` |
| 时钟 | `DetourCore/G.cs`：`DetourWatch.TimeStampMillis` |
| 角度回绕 | `DetourCore/LessMath.cs`：`normalizeTh` |
| 自转时 `l_step` 被加大 | `DetourCore/Algorithms/LidarOdometry.cs`（`lstepInc`） |
| 匹配成功后的位姿跳变 | `DetourCore/LocatorTypes/LidarMap.cs`（loop / relocalize） |
| 回环拧图 | `DetourCore/Algorithms/GraphOptimizer.cs` |
| 全局重定位入口 | `DetourCore/DetourLib.cs`：`Relocalize()` |
| 单位缩放 | `DetourCore/Configuration.cs`：`GuruOptions.inputScale / biasX / biasY` |

---

## 修订记录

- 2026-08-24：按当前 Detour 源码逐条答复原《Detour 信息确认清单》。