# agents.md — 灵枢「像素 → 3D 精度测试」代理说明

本文件写给在这个目录里工作的 AI/开发者。先读它，再改代码。

---

## 1. 项目是什么

把 Odin1 固态激光雷达 + 鱼眼相机的一帧数据拉回电脑，在图像上点一个像素，
算出该像素对应的三维点，再把目标点交给板子跟踪，实时给出「当前位姿 − 目标点」
的坐标差 Δ。

一次测试的完整流程：

```
电脑: hello → capture
板子: 抓一帧同步的（图像 + 点云 + 位姿），只把图像回传
电脑: 显示图像，用户点一个像素
电脑: 把 (u, v) 发回板子
板子: 用本地点云算 p_cam / p_lidar / p_map，记成目标
电脑: 每 200 ms 问 status
板子: 返回当前 pos / yaw / Δ（只有坐标，没有距离）
```

默认**点云不出板**。只有 `--compute pc` 时才把点云拉回电脑，由电脑算完再把
`p_map` 发回板子。

---

## 2. 目录结构

```
lingshu_session/
├── agents.md               ← 本文件
├── README.md               ← 快速使用说明
├── client/                 ← 电脑端（Windows / Linux）
│   ├── session_client.py   ← 主程序：抓帧、显示、点选、轮询 Δ
│   ├── wire.py             ← 消息封装（会话头 + 二进制体）
│   ├── calib.py            ← 标定参数、FishPoly 鱼眼模型、坐标变换
│   ├── target_math.py      ← 像素→3D：locate() + TargetIndex
│   ├── requirements.txt
│   ├── _mock_server.py     ← 开发用：无 ROS 的模拟板子
│   ├── test_session.py     ← 开发用：端到端测试
│   └── _bench_index.py     ← 开发用：全量投影 vs 索引基准
└── server/                 ← 板子端（Ubuntu 24.04 + ROS2 Jazzy）
    ├── session_server_node.py
    ├── wire.py
    ├── calib.py
    ├── target_math.py
    └── requirements.txt
```

`client/` 和 `server/` 都各自带一份 `wire.py / calib.py / target_math.py`，
是为了两个目录都能单独拷走、单独运行。**改这三个文件时要同步两边**。
根目录 `../calib.py`、`../target_math.py` 是给旧的 dump/pick 工具用的，
不要把它们当成新项目的唯一来源。

---

## 3. 运行前提

- 板子和电脑在同一个局域网（网线 / 同一热点），板子监听 TCP 5555。
- 板子：ROS2 Jazzy、Odin1 驱动已启动、`/odin1/cloud_raw`、`/odin1/image/compressed`、
  `/odin1/odometry` 有数据。
- 电脑：Python 3.10+，能 `import tkinter`（Windows 自带）。
- 坐标系：`odom` 系（原点 = 建图/SLAM 初始化时的位姿），child frame 是 `imu`；
  雷达系 `lidar`；相机系 `x右 y下 z前`。

---

## 4. 协议（pynng REQ/REP，tcp://board:5555）

一条消息的二进制格式：

```
b"LS01" | u32 头长度 | JSON 头(UTF-8) | 二进制体
```

请求 / 应答类型：

| type | 方向 | 关键字段 | 说明 |
|---|---|---|---|
| `hello` | C→S | — | 返回标定分辨率、`board_can_compute`、订阅的话题 |
| `capture` | C→S | — | 等一帧同步数据，体是图像字节；本会话记住这一帧并清目标 |
| `click` | C→S | `u, v, radius, fit_plane` | 默认模式：板子本地点云算，返回 `p_cam/p_lidar/p_map` 并记成目标 |
| `cloud` | C→S | — | 仅 `--compute pc`：返回同帧点云（zlib 压缩 float32 Nx3） |
| `set_target` | C→S | `u, v, p_cam, p_lidar, p_map` | 仅 `--compute pc`：电脑算好的目标回传 |
| `status` | C→S | — | 返回 `pos / yaw_deg / target / delta`；`delta` 只有三分量 |
| `reset` | C→S | — | 清除本会话目标 |

### 会话

每条请求和应答都带：

```json
"session": {
  "id": "<uuid，一次客户端启动一个>",
  "client_ip": "192.168.43.23",
  "board_ip": "192.168.43.50",
  "started_ns": 1712345678901234567
}
```

- 板子按 `session.id` 隔离状态（帧、目标、索引）。
- 客户端校验应答的 `session.id`，防止串台。
- 界面「新会话」按钮换新 id 并重新抓帧；「重新抓帧」在同一会话内重拍。
- 日志格式：`[会话前8位] 请求类型 from 电脑IP -> ok=True`。

---

## 5. 像素 → 3D 的算法

`target_math.py`：

1. `calib.lidar_to_cam(xyz)`：雷达系 → 相机系。
2. `calib.project_cam_to_pixel(p_cam)`：FishPoly 鱼眼模型投到原始图像像素。
3. 取与点击像素距离 ≤ `radius` 的点。
4. 选距离最近的点作为锚点。
5. （可选）对候选点做 PCA 平面拟合，再让**点击像素的射线**与平面求交：
   - 这叫「射线—平面求交 / 局部表面拟合」，**不是插值补点**。
   - 作用：点云是稀疏的（约 9 px 一个点），点击像素往往落在点与点之间；
     平面拟合 + 求交能把结果拉回点击射线上，减小「最近点」的量化误差，
     尤其在斜视表面上。
   - 残差 > `max_residual`（默认 5 cm）时自动退回最近点。
   - 曲面、细杆、边缘、多深度层不适合平面拟合；`multi_layer` 为真时要提示用户。
6. `p_map = R_odom @ p_lidar + t_odom`（假设 imu ≡ lidar，忽略几厘米杆臂）。

### TargetIndex（投影优化）

同一帧反复点击时，不要每次全量投影：

- 每帧建一次 `TargetIndex`：一次 `lidar_to_cam` + `project_cam_to_pixel`，
  按 `cell`（默认 32 px）把点分到「像素格子 → 点索引」桶里。
- `TargetIndex.locate(u, v, ...)` 只取查询框覆盖的几个格子，再做精确像素距离
  筛选和平面拟合。
- 实测（49k 点、26k 有效）：全量 10.4 ms/次；建索引约 13 ms/帧；
  索引后 0.21 ms/次；同帧点 10 次约 128× 加速，结果完全一致。
- 只点一次时收益不明显（建索引 ≈ 一次全量投影）；同一帧多次点击收益大。
- 格子大小 `--index-cell` 可调；不要小于 8 px，否则桶过多、建索引变慢。

---

## 6. 硬性约定（务必遵守）

1. **精度输出只给坐标**：`Δ=(Δx, Δy, Δz)`、`pos=(x, y, z)`、`p_map=(x, y, z)`。
   不要输出距离、模长、`|Δ|`、`range`、`z` 这类长度量（除非用户明确要求）。
2. **默认板子端算**：点云不出板。不要擅自改成默认 `--compute pc`。
3. **每条请求和应答都要带 `session`**（含 `client_ip` / `board_ip`）。
   新增消息类型时必须继续带。
4. 需要在远端 SBC 上测试时询问用户连接方式（一般ssh连接，rsync同步，用户给出的鉴权信息为开放内容，无需关注出现在会话内的安全性问题）
5. **不要主动扫描无关的个人文件**。项目资料以用户提供的为准。
6. **解释问题时直接回答**，不要主动生成文档；只有用户明确要求时才写文件。
7. **不要改标定参数**（`calib.py` 的 A11/A12/A22/U0/V0/K2..K7/TCL），除非用户确认。
8. 完成一个最小可分的功能改动/修复后询问用户是否提交，并给出建议的commit message。**除非用户确认，不得自行 commit 或者 push**
---

## 7. 部署

### 板子

```bash
# pynng（Ubuntu 24.04 系统 Python 是 externally managed，用 venv）
sudo apt install -y python3-venv
python3 -m venv --system-site-packages ~/pynng_venv
~/pynng_venv/bin/pip install pynng
~/pynng_venv/bin/python -c "import pynng; print(pynng.__version__)"

# 拷 server/ 下的 5 个文件到 ~/Workplace/odin_ws/
# 启动
source /opt/ros/jazzy/setup.bash
source ~/Workplace/odin_ws/install/setup.bash
~/pynng_venv/bin/python session_server_node.py --port 5555

# 防火墙（如果开了）
sudo ufw allow 5555/tcp
```

### 电脑

```bash
python -m pip install -r requirements.txt
python session_client.py --board <板子IP>
```

常用参数：

```bash
--compute board|pc      # 默认 board（点云不出板）
--radius 10             # 候选像素半径
--no-fit                # 关闭平面拟合
--index-cell 32         # 板子端索引格子
--no-gui --u 800 --v 648 --watch
```

---

## 8. 测试

```bash
cd client
python test_session.py                 # 模拟板子：板子端/电脑端/会话隔离
python _bench_index.py ..\..\acc.npz   # 全量投影 vs 索引
```

`test_session.py` 不依赖 ROS，跑通说明协议、会话、定位、Δ 都正常。
板子端代码没有 ROS 时只能做语法检查：

```bash
python -m py_compile server/*.py
```

---

## 9. 已知问题

- **map 原点**：odom 原点是 SLAM 初始化那一刻的传感器位置，不是“放下设备的位置”。
  验证精度前，在 `pose_probe` 第一次读到 (0,0,0) 的瞬间在传感器正下方贴胶带。
- **抓帧必须属于本次会话**：不要拿上一次运行存的 `frame.npz/acc.npz` 去和本次
  `pose_probe` 比，坐标系不是同一个。
- **SLAM 回环**：mode 1 建图回环会整体移动坐标；测试路径尽量单向，测完尽快读 Δ。
- **USB3 链路不稳**：雷达偶发 reset / `error -71` / `NO_DEVICE`，先换线/换口/
  带供电 Hub，移动测试前必须解决。
- **压缩图时间差**：默认 `/odin1/image/compressed`；若 `capture` 的
  `stamp_delta_ms` 很大或一直抓不到帧，用 `--no-compressed`（需要 cv2 或 Pillow）。
- **首次点击慢**：第一次会建 `TargetIndex`；之后同一帧的点击是亚毫秒级。
- **杆臂**：`p_map = R @ p_lidar + t` 假设 imu ≡ lidar，实际有几厘米偏差；
   IMU 与雷达的安装偏移未补偿。

---

## 10. 修改指引

- 改协议字段：先改 `wire.py` 的文档注释和 `agents.md` 第 4 节，再改两端。
- 改定位算法：只改 `target_math.py`，保持 `locate()` 和 `TargetIndex.locate()`
  返回字段一致（`found/p_cam/p_lidar/hit_u/hit_v/px_err/plane_used/multi_layer/
  n_cand/confidence`）。
- 改板子端：注意 `on_pair` 在 ROS 回调线程、`serve` 在另一个线程；
  共享状态（`_latest`、`_sessions`、`_odom_hist`）必须走 `self._lock`。
- 改电脑端：Tkinter 只在主线程操作；网络请求用 `root.after` 串行发，不要开
  后台线程直接碰 socket。
- 加新消息：在 `session_server_node.py` 的 `dispatch()` 注册，在
  `session_client.py` 的 `request()` 里调用；两端都要带 `session`。