LinkNexus/zhidian

Fork 0

Files

T

wangjingqiao 265f11aa38 initial commit

2026-10-22 09:07:39 +08:00

10 KiB

Raw Permalink Blame History

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	—	清除本会话目标

会话

每条请求和应答都带：

"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：

calib.lidar_to_cam(xyz)：雷达系 → 相机系。
calib.project_cam_to_pixel(p_cam)：FishPoly 鱼眼模型投到原始图像像素。
取与点击像素距离 ≤ radius 的点。
选距离最近的点作为锚点。
（可选）对候选点做 PCA 平面拟合，再让点击像素的射线与平面求交：
- 这叫「射线—平面求交 / 局部表面拟合」，不是插值补点。
- 作用：点云是稀疏的（约 9 px 一个点），点击像素往往落在点与点之间；平面拟合 + 求交能把结果拉回点击射线上，减小「最近点」的量化误差，尤其在斜视表面上。
- 残差 > max_residual（默认 5 cm）时自动退回最近点。
- 曲面、细杆、边缘、多深度层不适合平面拟合；multi_layer 为真时要提示用户。
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. 硬性约定（务必遵守）

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

7. 部署

板子

# 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

电脑

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

常用参数：

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

8. 测试

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

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

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。

10 KiB Raw Permalink Blame History Unescape Escape