243 lines
10 KiB
Markdown
243 lines
10 KiB
Markdown
# 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`。
|