commit 265f11aa3840502c57176c3e2972d7d26459418e Author: wangjingqiao Date: Thu Oct 22 09:07:39 2026 +0800 initial commit diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..aec06fd --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,242 @@ +# 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": "", + "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`。 diff --git a/README.md b/README.md new file mode 100644 index 0000000..d60a6c1 --- /dev/null +++ b/README.md @@ -0,0 +1,119 @@ +# 灵枢 · 会话式像素→3D 精度测试 + +在电脑上点一下图像，板子自动抓帧、算三维点、跟踪位姿，实时给出目标点的坐标差 Δ。 +不再手动 `scp` 图片、手动回传坐标。 + +## 目录 + +``` +lingshu_session/ +├── agents.md # 给 AI/开发者的项目说明与硬性约定 +├── README.md +├── client/ # 电脑端（Windows / Linux） +└── server/ # 板子端（Ubuntu 24.04 + ROS2 Jazzy） +``` + +## 快速开始 + +### 板子 + +```bash +# 1) pynng +sudo apt install -y python3-venv +python3 -m venv --system-site-packages ~/pynng_venv +~/pynng_venv/bin/pip install pynng + +# 2) 把 server/ 下 5 个文件拷到 ~/Workplace/odin_ws/ +# wire.py calib.py target_math.py session_server_node.py requirements.txt + +# 3) 启动驱动后，启动会话服务 +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 +cd client +python -m pip install -r requirements.txt +python session_client.py --board 192.168.43.50 +``` + +启动后自动抓帧并显示图像。点一个像素，右侧会显示： + +``` +p_cam（相机系） : (0.0611, -0.0917, 2.9978) +p_lidar（雷达系）: (3.0167, -0.0277, 0.1125) +★ p_map（地图系）: (3.0167, -0.0277, 0.1125) +``` + +移动板子，界面每 200 ms 刷新： + +``` +pos=(+0.9060, -0.0413, +0.0075) yaw=-3.2° +Δ=(+0.2507, -0.0209, +0.0144) +``` + +只给坐标，不给距离。 + +## 两种计算模式 + +- **默认 `--compute board`**：板子只回传图像；电脑只发 `(u,v)`； + 板子用本地点云算 `p_cam/p_lidar/p_map` 并记成目标。点云不出板。 +- **`--compute pc`**：板子额外回传同帧点云；电脑本地算，再 `set_target` 回传。 + 适合看候选点、平面拟合细节、离线复盘。 + +## 会话 + +- 一次客户端启动 = 一个会话（uuid + `client_ip` + `board_ip`）。 +- 每条请求和应答都带会话信息，板子按会话隔离状态。 +- 界面按钮：**新会话**（换 id 重抓）、**重新抓帧**（同会话重拍）、**清除目标**。 + +## 常用命令 + +```bash +# 无界面跑一次，看定位结果和 Δ +python session_client.py --board 192.168.43.50 --no-gui --u 800 --v 648 --watch + +# 电脑端计算 +python session_client.py --board 192.168.43.50 --compute pc --no-gui --u 800 --v 648 + +# 调候选半径 / 关平面拟合 +python session_client.py --board 192.168.43.50 --radius 15 --no-fit +``` + +板子端参数： + +```bash +--image-topic /odin1/image/compressed # 默认，直接转发，不用编码 +--no-compressed # 原始图，需要 cv2 或 Pillow +--cloud-topic /odin1/cloud_raw +--odom-topic /odin1/odometry +--index-cell 32 # 投影索引格子大小 +--radius 10 # 默认候选半径 +``` + +## 测试（不需要 ROS） + +```bash +cd client +python test_session.py # 协议 / 两种计算模式 / 会话隔离 +python _bench_index.py ..\..\acc.npz # 全量投影 vs 像素索引 +``` + +## 常见问题 + +| 现象 | 处理 | +|---|---| +| `capture` 一直失败/超时 | 板子驱动是否在跑；5555 是否放行；`/odin1/cloud_raw` 是否有数据 | +| 抓到的图像很旧 | `capture` 的 `stamp_delta_ms` 是否很大；改用 `--no-compressed` | +| 点不到点 | 调大 `--radius`；换平整、点云密的区域；看 `multi_layer` / `plane_used` | +| `Δ` 一直不为 0 | 先确认站到了目标点；再查 map 原点胶带、抓帧是否属于本次会话 | +| 板子端报 `target_math.py` 缺失 | `server/` 下的 5 个文件没有拷全 | +| `pynng` 装不上 | 用 `--system-site-packages` 的 venv；或装 cmake/build-essential 从源码编 | + +更细的协议、坐标约定、硬性规则见 `agents.md`。 diff --git a/client/_bench_index.py b/client/_bench_index.py new file mode 100644 index 0000000..723bc94 --- /dev/null +++ b/client/_bench_index.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""_bench_index.py — 对比全量投影 locate() 与网格索引 TargetIndex 的耗时。""" +import time +import numpy as np + +import target_math + + +def bench(fn, n): + t0 = time.perf_counter() + for _ in range(n): + fn() + return (time.perf_counter() - t0) / n * 1000.0 + + +def main(path='acc.npz'): + data = np.load(path) + xyz = data['xyz'] + conf = data['confidence'] if 'confidence' in data else None + valid = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6) + print('frame: %s N=%d valid=%d (%.1f%%)' + % (path, xyz.shape[0], int(valid.sum()), 100.0 * valid.mean())) + + # 找一个确实能命中的像素 + query = None + for u in range(700, 901, 20): + for v in range(550, 751, 20): + r = target_math.locate(xyz, u, v, conf=conf, radius=10) + if r['found']: + query = (float(u), float(v), r) + break + if query: + break + if query is None: + raise SystemExit('没找到可命中的像素') + u, v, r0 = query + print('query pixel: (%.0f, %.0f) p_lidar=%s plane=%s cand=%d' + % (u, v, np.round(r0['p_lidar'], 4), r0['plane_used'], r0['n_cand'])) + + n_click = 50 + t_full = bench(lambda: target_math.locate(xyz, u, v, conf=conf, radius=10), n_click) + print('\n全量 locate(): %.3f ms/次（%d 次平均）' % (t_full, n_click)) + + for cell in (16, 32, 64): + t0 = time.perf_counter() + idx = target_math.TargetIndex(xyz, conf=conf, cell=cell) + build = (time.perf_counter() - t0) * 1000.0 + r1 = idx.locate(u, v, radius=10) + t_idx = bench(lambda: idx.locate(u, v, radius=10), n_click) + same = (r1['found'] == r0['found']) + dp = None + if r1['found'] and r0['found']: + dp = np.linalg.norm(np.asarray(r1['p_lidar']) - np.asarray(r0['p_lidar'])) + print('cell=%2d px 建索引 %6.2f ms | 每次 %6.3f ms | 桶=%4d | 候选=%d | 结果一致=%s | Δp_lidar=%s' + % (cell, build, t_idx, len(idx.buckets), r1['n_cand'], same, + '%.2e' % dp if dp is not None else '-')) + + # 模拟同一帧点 10 次（不同像素） + rng = np.random.default_rng(0) + pixels = [(u + rng.uniform(-60, 60), v + rng.uniform(-60, 60)) for _ in range(10)] + + def ten_full(): + for uu, vv in pixels: + target_math.locate(xyz, uu, vv, conf=conf, radius=10) + + idx = target_math.TargetIndex(xyz, conf=conf, cell=32) + idx.locate(u, v) # 预热 + + def ten_idx(): + for uu, vv in pixels: + idx.locate(uu, vv, radius=10) + + t10_full = bench(ten_full, 10) + t10_idx = bench(ten_idx, 10) + print('\n同一帧点 10 次: 全量 %.2f ms | 索引 %.2f ms | 加速 %.1fx' + % (t10_full, t10_idx, t10_full / max(t10_idx, 1e-9))) + + # 半径 20 的情况 + t20_idx = bench(lambda: idx.locate(u, v, radius=20), n_click) + print('索引 radius=20: %.3f ms/次' % t20_idx) + + +if __name__ == '__main__': + import sys + main(sys.argv[1] if len(sys.argv) > 1 else 'acc.npz') diff --git a/client/_mock_server.py b/client/_mock_server.py new file mode 100644 index 0000000..a1f11d6 --- /dev/null +++ b/client/_mock_server.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +_mock_server.py — 仅用于在电脑上测试协议，不需要 ROS。 + +它模拟板子端：返回一张合成图像、一片合成点云（3 m 处、像素 800,648 附近）， +以及一个会随时间变化的位姿，用来验证 session_client 的抓帧、点选、回传、Δ 显示。 +""" +import io +import threading +import time +import zlib + +import numpy as np +from PIL import Image +import pynng + +import calib +import target_math +import wire + +CAL_W, CAL_H = calib.IMAGE_WIDTH, calib.IMAGE_HEIGHT +U0, V0 = 800.0, 648.0 +DEPTH = 3.0 + + +def build_cloud(): + pts = [] + for du in range(-12, 13, 4): + for dv in range(-12, 13, 4): + ray = calib.pixel_to_ray(U0 + du, V0 + dv) + p_cam = ray * DEPTH + pts.append(calib.cam_to_lidar(p_cam)[0]) + pts = np.asarray(pts, dtype=np.float32) + # 加一些无效点，模拟真实点云的零值 + zeros = np.zeros((97, 3), dtype=np.float32) + return np.concatenate([pts, zeros], axis=0) + + +def build_image(): + arr = np.zeros((CAL_H, CAL_W, 3), dtype=np.uint8) + arr[:, :, 2] = 40 + u, v = int(round(U0)), int(round(V0)) + arr[v - 6:v + 6, u - 6:u + 6] = (255, 255, 255) + b = io.BytesIO() + Image.fromarray(arr).save(b, format='JPEG', quality=85) + return b.getvalue() + + +class MockServer: + def __init__(self, port=5599): + self.port = port + self.xyz = build_cloud() + self.image = build_image() + self.target = None + self.index = None + self.sessions = {} + self.stop_event = threading.Event() + self.thread = None + + def start(self): + self.thread = threading.Thread(target=self._serve, daemon=True) + self.thread.start() + + def stop(self): + self.stop_event.set() + if self.thread: + self.thread.join(timeout=2) + + def _odom(self): + return { + 'stamp_ns': time.time_ns(), + 'position': [0.0, 0.0, 0.0], + 'orientation_xyzw': [0.0, 0.0, 0.0, 1.0], + 'frame_id': 'odom', + 'child_frame_id': 'imu', + 'delta_ms': 1.0, + } + + def _handle(self, hdr): + typ = hdr.get('type') + if typ == 'hello': + return {'ok': True, 'type': 'hello', 'node': 'mock', + 'calib_width': CAL_W, 'calib_height': CAL_H, + 'board_can_compute': True, + 'compute_modes': ['board', 'pc'], + 'topics': {'image': 'mock', 'cloud': 'mock', 'odom': 'mock'}}, b'' + if typ == 'capture': + sid = (hdr.get('session') or {}).get('id') + self.sessions[sid] = {'target': None} + return {'ok': True, 'type': 'frame', 'image_format': 'jpeg', + 'image_bytes': len(self.image), + 'image_width': CAL_W, 'image_height': CAL_H, + 'calib_width': CAL_W, 'calib_height': CAL_H, + 'image_stamp_ns': time.time_ns(), + 'cloud_stamp_ns': time.time_ns(), + 'stamp_delta_ms': 1.2, + 'n_points': int(self.xyz.shape[0]), + 'board_can_compute': True, + 'odom': self._odom()}, self.image + if typ == 'click': + sid = (hdr.get('session') or {}).get('id') + s = self.sessions.get(sid) + if s is None: + return {'ok': False, 'type': 'target', 'error': '先 capture'}, b'' + if self.index is None: + self.index = target_math.TargetIndex(self.xyz) + u = float(hdr['u']) + v = float(hdr['v']) + res = self.index.locate(u, v, + radius=float(hdr.get('radius', 10.0)), + fit_plane=bool(hdr.get('fit_plane', True))) + out = {'ok': True, 'type': 'target', 'found': bool(res['found']), + 'u': u, 'v': v, 'n_cand': int(res['n_cand']), + 'plane_used': bool(res['plane_used']), + 'multi_layer': bool(res['multi_layer'])} + if res['found']: + out.update({ + 'p_cam': [float(x) for x in res['p_cam']], + 'p_lidar': [float(x) for x in res['p_lidar']], + 'hit_u': float(res['hit_u']), 'hit_v': float(res['hit_v']), + 'px_err': float(res['px_err']), + 'confidence': None, + }) + pm = np.asarray(res['p_lidar'], dtype=np.float64) + out['p_map'] = [float(x) for x in pm] + out['odom'] = self._odom() + s['target'] = pm + return out, b'' + if typ == 'cloud': + body = zlib.compress(self.xyz.astype(' r = sqrt(x²+y²) -> θ = atan(r) + θ_d = θ + k2θ² + k3θ³ + k4θ⁴ + k5θ⁵ + k6θ⁶ + k7θ⁷ + 畸变后的归一化坐标 = (x/r, y/r) * θ_d + 像素 = (A11·x_d + A12·y_d + u0, A22·y_d + v0) + +注意：这里所有"像素"都指**原始（未去畸变）图像**的像素，也就是 /odin1/image。 +""" +import numpy as np + +# ---------------- calib.yaml（设备 #O1-N090100083） ---------------- +A11 = 730.51652555414909 +A12 = -0.68917984297115186 +A22 = 730.30384140813805 +U0 = 785.10214573595919 +V0 = 670.33088344368650 + +K2 = -8.2510665959742261e-03 +K3 = 1.6324575756549702e-02 +K4 = -5.6837005957061821e-02 +K5 = 4.9991959834060670e-02 +K6 = -2.4463726125325536e-02 +K7 = 3.8528646040351232e-03 + +IMAGE_WIDTH = 1600 +IMAGE_HEIGHT = 1296 +MAX_INCIDENT_DEG = 120.0 + +# Tcl_0：雷达 -> 相机 +TCL = np.array([ + [-0.00177, -0.99998, 0.00586, 0.03809], + [ 0.00744, -0.00588, -0.99996, -0.00189], + [ 0.99997, -0.00173, 0.00745, -0.01966], + [ 0.0, 0.0, 0.0, 1.0 ], +], dtype=np.float64) +R_CAM_LIDAR = TCL[:3, :3] +T_CAM_LIDAR = TCL[:3, 3] + +# 安全上限：θ 接近 90° 时 tan(θ) 发散。 +# 这颗镜头水平半视角约 63°，图像四角可到 ~87°，所以上限放到 89°。 +_THETA_MAX = np.deg2rad(89.0) + + +def theta_d(theta): + """把入射角 θ 映射成畸变后的归一化半径。""" + th = np.asarray(theta, dtype=np.float64) + return (th + K2 * th**2 + K3 * th**3 + K4 * th**4 + + K5 * th**5 + K6 * th**6 + K7 * th**7) + + +def dtheta_d(theta): + """θ_d 对 θ 的导数（牛顿迭代用）。""" + th = np.asarray(theta, dtype=np.float64) + return (1.0 + 2 * K2 * th + 3 * K3 * th**2 + 4 * K4 * th**3 + + 5 * K5 * th**4 + 6 * K6 * th**5 + 7 * K7 * th**6) + + +def solve_theta(rho_d, iters=30): + """反解：给定畸变归一化半径 rho_d，求入射角 θ。""" + rho_d = np.asarray(rho_d, dtype=np.float64) + th = np.minimum(rho_d, _THETA_MAX).astype(np.float64) + for _ in range(iters): + f = theta_d(th) - rho_d + fp = dtheta_d(th) + fp = np.where(np.abs(fp) < 1e-12, 1e-12, fp) + step = f / fp + th = np.clip(th - step, 0.0, _THETA_MAX) + if np.all(np.abs(step) < 1e-12): + break + return th + + +def pixel_to_ray(u, v): + """原始(畸变)图像像素 -> 相机系下的单位方向向量。 + + 标量输入返回 (3,)，数组输入返回 (N,3)。 + """ + u = np.asarray(u, dtype=np.float64) + v = np.asarray(v, dtype=np.float64) + yd = (v - V0) / A22 + xd = (u - U0 - A12 * yd) / A11 + + rho = np.hypot(xd, yd) + small = rho < 1e-9 + rho_safe = np.where(small, 1.0, rho) + + theta = solve_theta(rho) + r = np.tan(theta) + + x = xd * (r / rho_safe) + y = yd * (r / rho_safe) + d = np.stack([x, y, np.ones_like(x)], axis=-1) + d = d / np.linalg.norm(d, axis=-1, keepdims=True) + return d + + +def project_cam_to_pixel(p_cam, margin=0.0): + """相机系点 (N,3) -> 原始图像像素 (u, v, valid)。""" + p = np.atleast_2d(np.asarray(p_cam, dtype=np.float64)) + Z = p[:, 2] + valid = np.isfinite(p).all(axis=1) & (Z > 1e-6) + Zs = np.where(valid, Z, 1.0) + + x = p[:, 0] / Zs + y = p[:, 1] / Zs + r = np.hypot(x, y) + theta = np.arctan(r) + thd = theta_d(theta) + + s = np.where(r > 1e-9, thd / np.where(r > 1e-9, r, 1.0), 1.0) + xd = x * s + yd = y * s + + u = A11 * xd + A12 * yd + U0 + v = A22 * yd + V0 + valid &= np.isfinite(u) & np.isfinite(v) + valid &= (u >= -margin) & (u <= IMAGE_WIDTH - 1 + margin) + valid &= (v >= -margin) & (v <= IMAGE_HEIGHT - 1 + margin) + return u, v, valid + + +def lidar_to_cam(pts): + """雷达系点 (N,3) -> 相机系点 (N,3)。""" + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + return pts @ R_CAM_LIDAR.T + T_CAM_LIDAR + + +def cam_to_lidar(pts): + """相机系点 (N,3) -> 雷达系点 (N,3)。""" + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + return (pts - T_CAM_LIDAR) @ R_CAM_LIDAR + + +def cam_to_base_link(pts, cam_xyz=(0.0, 0.0, 0.0), rpy_deg=(0.0, 0.0, 0.0)): + """相机系 -> 车体系。 + + 坐标约定： + 相机系：x 右, y 下, z 前 + 车体系：x 前, y 左, z 上（ROS REP-103, base_link） + + 参数： + cam_xyz : 相机光心在车体系下的位置 (m)，例如 (0.1, 0.0, 0.3) + rpy_deg : 相机相对车体的姿态 (roll, pitch, yaw)，单位度 + pitch > 0 表示镜头向下俯（常见安装） + """ + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + R = _base_from_cam_rotation(rpy_deg) + return pts @ R.T + np.asarray(cam_xyz, dtype=np.float64) + + +def base_link_to_cam(pts, cam_xyz=(0.0, 0.0, 0.0), rpy_deg=(0.0, 0.0, 0.0)): + """车体系 -> 相机系（cam_to_base_link 的逆）。""" + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + R = _base_from_cam_rotation(rpy_deg) + return (pts - np.asarray(cam_xyz, dtype=np.float64)) @ R + + +def _base_from_cam_rotation(rpy_deg=(0.0, 0.0, 0.0)): + """构造 R_base_cam。零姿态时：相机 z(前)->车体 x(前)，x(右)->车体 -y，y(下)->车体 -z。""" + roll, pitch, yaw = np.deg2rad(rpy_deg) + Rx = np.array([[1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)]]) + Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)]]) + Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1]]) + R0 = np.array([[0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0]]) + return Rz @ Ry @ Rx @ R0 + + +if __name__ == '__main__': + # 往返一致性自测：像素 -> 射线 -> 投回像素 + print('Tcl determinant (should be +1):', np.linalg.det(R_CAM_LIDAR)) + print('camera position in lidar frame (m):', cam_to_lidar(np.zeros((1, 3)))[0]) + print() + for u, v in [(800, 648), (100, 100), (1500, 1200), (400, 900), (1599, 0)]: + d = pixel_to_ray(u, v) + p = d * 3.0 + u2, v2, ok = project_cam_to_pixel(p) + print('pixel (%4d,%4d) -> ray %s -> back (%8.2f,%8.2f) valid=%s err=%.2e' + % (u, v, np.round(d, 4), u2[0], v2[0], ok[0], + np.hypot(u2[0] - u, v2[0] - v))) diff --git a/client/requirements.txt b/client/requirements.txt new file mode 100644 index 0000000..92c2bc6 --- /dev/null +++ b/client/requirements.txt @@ -0,0 +1,7 @@ +# 电脑端（Windows / Linux） +pynng>=0.9.0 +numpy>=1.26 +Pillow>=10.0 + +# Tkinter 随 Python 自带（Windows 安装包默认包含）。 +# Linux 如果缺：sudo apt install python3-tk diff --git a/client/session_client.py b/client/session_client.py new file mode 100644 index 0000000..b0a6678 --- /dev/null +++ b/client/session_client.py @@ -0,0 +1,572 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +session_client.py — 电脑端：一次运行 = 一个会话。 + +流程全自动，不再手动 scp 图片/坐标： + 1. 启动 -> hello（握手）-> capture（板子抓帧，只回传图像） + 2. 图像显示在窗口里；鼠标点一个像素 + 3. 只把 (u, v) 发给板子；板子用本地缓存的同帧点云算 + p_cam / p_lidar / p_map，并把 p_map 记成目标（点云不出板） + 4. 你移动板子，界面每 200 ms 问一次 status，实时显示 Δ 坐标（只给坐标，不给距离） + + 需要电脑端算（要看候选点/平面拟合细节、离线复盘）时： + python session_client.py --board 192.168.43.50 --compute pc + 这时才会把同帧点云拉回电脑，算完把 p_map 发回板子。 + +用法（电脑）： + python session_client.py --board 192.168.43.50 + python session_client.py --board 192.168.43.50 --radius 12 + python session_client.py --board 192.168.43.50 --no-gui --u 800 --v 648 --watch + +依赖：pynng、numpy、Pillow（Tkinter 随 Python 自带） +""" +import argparse +import csv +import io +import os +import socket +import time +import zlib + +import numpy as np +from PIL import Image, ImageTk +import tkinter as tk + +import pynng + +import target_math +import wire + +MAX_W, MAX_H = 1100, 800 + + +# --------------------------------------------------------------------- 网络 +def detect_local_ip(board_ip, port): + """问内核：去板子的路由走哪张网卡，返回本机在这个网段上的 IP。""" + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + s.connect((board_ip, port)) # UDP connect 不发包，只选路 + return s.getsockname()[0] + finally: + s.close() + + +class BoardClient: + def __init__(self, board_ip, port, timeout_ms=8000): + self.board_ip = board_ip + self.port = int(port) + self.client_ip = detect_local_ip(board_ip, self.port) + self.session = wire.new_session(board_ip, self.client_ip) + self.sock = pynng.Req0(dial='tcp://%s:%d' % (board_ip, self.port)) + self.sock.recv_max_size = 32 * 1024 * 1024 + self.sock.send_timeout = 5000 + self.sock.recv_timeout = int(timeout_ms) + + def request(self, typ, blob=b'', **fields): + msg = {'session': self.session, 'type': typ, 'ts_ns': time.time_ns()} + msg.update(fields) + self.sock.send(wire.pack(msg, blob)) + hdr, out_blob = wire.unpack(self.sock.recv()) + wire.check_session(hdr, self.session) + return hdr, out_blob + + def new_session(self, note=''): + """开始一个新会话（新的 id，仍然带同一个 client_ip / board_ip）。""" + self.session = wire.new_session(self.board_ip, self.client_ip, note) + return self.session + + def close(self): + try: + self.sock.close() + except Exception: + pass + + +# --------------------------------------------------------------------- 计算 +def quat_to_rot(q): + x, y, z, w = [float(v) for v in q] + return np.array([ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ], dtype=np.float64) + + +def decode_cloud(hdr, blob): + if not hdr.get('ok'): + raise RuntimeError(hdr.get('error', 'cloud 请求失败')) + if hdr.get('encoding') != 'zlib_f32': + raise RuntimeError('未知点云编码: %s' % hdr.get('encoding')) + raw = zlib.decompress(blob) + n = int(hdr.get('n_points', 0)) + return np.frombuffer(raw, dtype=' (locate 结果, map 系坐标)。u/v 用标定分辨率(1600x1296)。""" + if index is not None: + res = index.locate(u, v, radius=radius, fit_plane=fit_plane) + else: + res = target_math.locate(xyz, u, v, radius=radius, fit_plane=fit_plane) + p_map = None + if res['found'] and odom: + R = quat_to_rot(odom['orientation_xyzw']) + t = np.asarray(odom['position'], dtype=np.float64) + p_map = R @ np.asarray(res['p_lidar'], dtype=np.float64) + t + return res, p_map + + +def fmt3(p): + return '(%.4f, %.4f, %.4f)' % (float(p[0]), float(p[1]), float(p[2])) + + +def result_text(res, p_map, u, v): + if not res['found']: + return ('像素(标定系): (%.1f, %.1f)\n\n' + '未找到点云（候选 %d）\n' + '可能：无回波 / 超出视场 / 半径太小\n' + '试试把半径调大（如 15~20），或点平整、点云密的区域' + % (u, v, res.get('n_cand', 0))) + + lines = [ + '像素(标定系): (%.1f, %.1f)' % (u, v), + '命中像素: (%.1f, %.1f) 偏离 %.2f px' % (res['hit_u'], res['hit_v'], res['px_err']), + '候选数: %d' % res['n_cand'], + '平面拟合: %s' % ('是' if res['plane_used'] else '否'), + ] + if res.get('multi_layer'): + lines.append('! 候选跨多个深度层，可能擦过边缘') + if res.get('confidence') is not None: + lines.append('置信度: %.0f' % res['confidence']) + lines += [ + '', + 'p_cam（相机系） : %s' % fmt3(res['p_cam']), + 'p_lidar（雷达系）: %s' % fmt3(res['p_lidar']), + ] + if p_map is not None: + lines.append('★ p_map（地图系）: %s' % fmt3(p_map)) + lines.append(' （map 原点 = 建图初始位置）') + else: + lines.append('p_map: 不可用（本帧没有 SLAM 位姿）') + return '\n'.join(lines) + + +# --------------------------------------------------------------------- 界面 +class App: + def __init__(self, root, cli, args): + self.root = root + self.cli = cli + self.args = args + self.image = None + self.img_w = self.img_h = 0 + self.calib_w = self.calib_h = 0 + self.scale = 1.0 + self.xyz = None + self.index = None + self.capture_odom = None + self.target = None + self.poll_job = None + self.compute_mode = args.compute + try: + hello, _ = self.cli.request('hello') + if not hello.get('board_can_compute'): + if self.compute_mode == 'board': + print('板子不能本地计算，自动切到 --compute pc') + self.compute_mode = 'pc' + except Exception as exc: + print('hello 失败: %s' % exc) + self._build_ui() + self.capture() + + # ---------------------------------------------------------------- UI + def _build_ui(self): + self.root.title('灵枢 · 会话式精度测试') + + left = tk.Frame(self.root) + left.pack(side=tk.LEFT, padx=8, pady=8) + right = tk.Frame(self.root) + right.pack(side=tk.RIGHT, fill=tk.Y, padx=8, pady=8) + + self.canvas = tk.Canvas(left, width=MAX_W, height=MAX_H, + cursor='crosshair', bg='#111') + self.canvas.pack() + self.canvas.bind('', self.on_click) + + bar = tk.Frame(right) + bar.pack(anchor='w', fill=tk.X) + tk.Button(bar, text='新会话', command=self.new_session).pack(side=tk.LEFT) + tk.Button(bar, text='重新抓帧', command=self.capture).pack(side=tk.LEFT, padx=6) + tk.Button(bar, text='清除目标', command=self.reset_target).pack(side=tk.LEFT) + + pf = tk.Frame(right) + pf.pack(anchor='w', pady=6) + tk.Label(pf, text='候选半径(px):').pack(side=tk.LEFT) + self.radius_var = tk.StringVar(value=str(self.args.radius)) + tk.Entry(pf, textvariable=self.radius_var, width=6).pack(side=tk.LEFT) + self.fit_var = tk.BooleanVar(value=not self.args.no_fit) + tk.Checkbutton(pf, text='平面拟合', variable=self.fit_var).pack(side=tk.LEFT, padx=8) + + self.text = tk.Text(right, width=58, height=18, font=('Consolas', 10)) + self.text.pack(fill=tk.BOTH, expand=True) + + tk.Label(right, text='实时位姿 / 误差（只给坐标）', + anchor='w').pack(fill=tk.X, pady=(8, 0)) + self.status_var = tk.StringVar(value='等待抓帧…') + tk.Label(right, textvariable=self.status_var, justify='left', anchor='w', + font=('Consolas', 12), fg='#0a7').pack(fill=tk.X) + + self.info_var = tk.StringVar(value='') + tk.Label(right, textvariable=self.info_var, justify='left', anchor='w', + font=('Consolas', 9), fg='#666').pack(fill=tk.X, pady=(6, 0)) + + def set_text(self, s): + self.text.configure(state=tk.NORMAL) + self.text.delete('1.0', tk.END) + self.text.insert(tk.END, s) + self.text.configure(state=tk.DISABLED) + + def set_status(self, s): + self.status_var.set(s) + + # ---------------------------------------------------------------- 抓帧 + def capture(self): + self.cancel_poll() + self.target = None + self.set_status('正在抓帧…') + self.root.update_idletasks() + + try: + h, img_blob = self.cli.request('capture') + except Exception as exc: + self.set_status('抓帧失败: %s' % exc) + return + if not h.get('ok'): + self.set_status('抓帧失败: %s' % h.get('error')) + return + + try: + im = Image.open(io.BytesIO(img_blob)).convert('RGB') + except Exception as exc: + self.set_status('图像解码失败: %s' % exc) + return + + self.image = im + self.img_w, self.img_h = im.size + self.calib_w = int(h.get('calib_width') or self.img_w) + self.calib_h = int(h.get('calib_height') or self.img_h) + + # 默认板子端算，点云不出板；只有 --compute pc 才拉同帧点云 + self.xyz = None + self.index = None + cloud_err = None + if self.compute_mode == 'pc': + try: + hc, cblob = self.cli.request('cloud') + self.xyz = decode_cloud(hc, cblob) + except Exception as exc: + cloud_err = str(exc) + + self.capture_odom = h.get('odom') + + # 显示 + self.scale = min(MAX_W / self.img_w, MAX_H / self.img_h, 1.0) + disp = im.resize((max(1, int(self.img_w * self.scale)), + max(1, int(self.img_h * self.scale))), Image.LANCZOS) + self.tkimg = ImageTk.PhotoImage(disp) + self.canvas.config(width=disp.width, height=disp.height) + self.canvas.delete('all') + self.canvas.create_image(0, 0, anchor='nw', image=self.tkimg) + + odom = self.capture_odom + info = [ + '会话 : %s' % self.cli.session['id'][:8], + '链路 : %s -> %s:%d' % (self.cli.client_ip, self.cli.board_ip, self.cli.port), + '图像 : %dx%d（标定 %dx%d）' % (self.img_w, self.img_h, + self.calib_w, self.calib_h), + '计算 : %s' % ('板子端（点云不出板）' if self.compute_mode == 'board' + else '电脑端（已拉回点云）'), + '点云 : %s' % (('%d 点（已拉回）' % self.xyz.shape[0]) + if self.xyz is not None + else ('留在板子' if self.compute_mode == 'board' + else '获取失败: %s' % cloud_err)), + '图像/点云: %.2f ms' % float(h.get('stamp_delta_ms') or 0.0), + ] + if odom: + info.append('抓帧位姿 : %s' % fmt3(odom['position'])) + else: + info.append('抓帧位姿 : 无（板子没给 odom）') + info.append('') + info.append('点击图像选择目标点…') + self.set_text('\n'.join(info)) + self.info_var.set('会话 %s | %s -> %s' % ( + self.cli.session['id'][:8], self.cli.client_ip, self.cli.board_ip)) + self.set_status('已抓帧，请点选目标') + + # ---------------------------------------------------------------- 点选 + def on_click(self, event): + if self.image is None: + self.set_status('还没有图像，先抓帧') + return + if self.compute_mode == 'pc' and self.xyz is None: + self.set_status('点云没拉到，无法本地计算；可改用默认的板子端计算') + return + + u_img = event.x / self.scale + v_img = event.y / self.scale + u_cal = u_img * self.calib_w / self.img_w + v_cal = v_img * self.calib_h / self.img_h + + try: + radius = float(self.radius_var.get()) + except ValueError: + radius = self.args.radius + fit_plane = bool(self.fit_var.get()) + + if self.compute_mode == 'board': + # 点云不出板：只发像素，板子算完把 p_map 记成目标 + try: + res, _ = self.cli.request('click', u=u_cal, v=v_cal, + radius=radius, fit_plane=fit_plane) + except Exception as exc: + self.set_status('点选失败: %s' % exc) + return + p_map = res.get('p_map') + else: + # 电脑端算：先拉点云，本地算，再把 p_map 回传板子 + try: + if self.index is None: + self.index = target_math.TargetIndex(self.xyz) + res, p_map = compute_target(self.xyz, self.capture_odom, + u_cal, v_cal, radius, fit_plane, + index=self.index) + except Exception as exc: + self.set_status('计算失败: %s' % exc) + return + if res['found'] and p_map is not None: + try: + h, _ = self.cli.request( + 'set_target', u=u_cal, v=v_cal, + p_cam=[float(x) for x in res['p_cam']], + p_lidar=[float(x) for x in res['p_lidar']], + p_map=[float(x) for x in p_map]) + except Exception as exc: + self.set_status('回传目标失败: %s' % exc) + return + if not h.get('ok'): + self.set_status('板子拒绝目标: %s' % h.get('error')) + return + + self.draw_marker(event.x, event.y) + self.set_text(result_text(res, p_map, u_cal, v_cal)) + + if not res.get('found'): + self.set_status('没找到点，换个位置或调大半径') + return + if p_map is None: + self.set_status('找到了点，但本帧没有位姿，无法跟踪') + return + + self.target = p_map + self.set_status('目标已锁定，移动板子…') + self.log_row(u_cal, v_cal, res, p_map) + self.schedule_poll() + + def draw_marker(self, x, y): + self.canvas.delete('marker') + self.canvas.create_oval(x - 7, y - 7, x + 7, y + 7, + outline='#00ff66', width=2, tags='marker') + self.canvas.create_line(x - 15, y, x + 15, y, fill='#00ff66', tags='marker') + self.canvas.create_line(x, y - 15, x, y + 15, fill='#00ff66', tags='marker') + + # ---------------------------------------------------------------- 轮询 + def schedule_poll(self): + self.cancel_poll() + self.poll_job = self.root.after(self.args.poll_ms, self.poll_status) + + def cancel_poll(self): + if self.poll_job is not None: + try: + self.root.after_cancel(self.poll_job) + except Exception: + pass + self.poll_job = None + + def poll_status(self): + self.poll_job = None + if self.target is None: + return + try: + h, _ = self.cli.request('status') + except Exception as exc: + self.set_status('status 失败: %s' % exc) + self.schedule_poll() + return + + if not h.get('ok') or not h.get('valid'): + self.set_status('等待里程计…（%s）' % h.get('error', '位姿停更')) + else: + pos = h['pos'] + lines = ['pos=(%+.4f, %+.4f, %+.4f) yaw=%+.1f°' + % (pos[0], pos[1], pos[2], h.get('yaw_deg', 0.0))] + d = h.get('delta') + if d: + lines.append('Δ=(%+.4f, %+.4f, %+.4f)' % (d[0], d[1], d[2])) + self.set_status('\n'.join(lines)) + self.schedule_poll() + + # ---------------------------------------------------------------- 其它 + def reset_target(self): + self.cancel_poll() + self.target = None + self.set_status('已清除目标') + try: + self.cli.request('reset') + except Exception: + pass + + def new_session(self): + """开一个新会话：新 id + 重新抓帧（每次测试互不干扰）。""" + self.cancel_poll() + self.target = None + self.cli.new_session() + self.capture() + + def log_row(self, u, v, res, p_map): + if self.args.no_log: + return + path = self.args.log + new = not os.path.exists(path) + with open(path, 'a', newline='', encoding='utf-8-sig') as f: + w = csv.writer(f) + if new: + w.writerow(['session_id', 'client_ip', 'board_ip', + 'u_cal', 'v_cal', + 'p_cam_x', 'p_cam_y', 'p_cam_z', + 'p_lidar_x', 'p_lidar_y', 'p_lidar_z', + 'p_map_x', 'p_map_y', 'p_map_z', + 'plane_used', 'n_cand', 'px_err', 'ts_ns']) + w.writerow([ + self.cli.session['id'], self.cli.client_ip, self.cli.board_ip, + round(u, 2), round(v, 2)] + + [round(float(res['p_cam'][i]), 4) for i in range(3)] + + [round(float(res['p_lidar'][i]), 4) for i in range(3)] + + [round(float(p_map[i]), 4) for i in range(3)] + + [int(res['plane_used']), int(res['n_cand']), + round(float(res['px_err']), 2), time.time_ns()]) + + +# --------------------------------------------------------------------- 无界面 +def print_status(h): + if not h.get('ok'): + print('status: %s' % h.get('error')) + return + pos = h.get('pos') + line = ('pos=(%+.4f, %+.4f, %+.4f) yaw=%+.1f°' + % (pos[0], pos[1], pos[2], h.get('yaw_deg', 0.0))) if pos else 'pos=无' + if not h.get('valid'): + line += ' [位姿无效]' + d = h.get('delta') + if d: + line += '\nΔ=(%+.4f, %+.4f, %+.4f)' % (d[0], d[1], d[2]) + print(line, flush=True) + + +def run_cli(cli, args): + hello, _ = cli.request('hello') + mode = args.compute + if mode == 'board' and not hello.get('board_can_compute'): + print('板子不能本地计算，自动切到 --compute pc', flush=True) + mode = 'pc' + print('hello: ok=%s node=%s calib=%sx%s compute=%s' + % (hello.get('ok'), hello.get('node'), hello.get('calib_width'), + hello.get('calib_height'), mode), flush=True) + + h, img_blob = cli.request('capture') + if not h.get('ok'): + raise SystemExit('capture 失败: %s' % h.get('error')) + print('capture: %d B, %sx%s, 点云 %d 点, 时间差 %.2f ms' + % (len(img_blob), h.get('image_width'), h.get('image_height'), + h.get('n_points', 0), h.get('stamp_delta_ms', 0.0)), flush=True) + if h.get('odom'): + print('抓帧位姿: %s' % fmt3(h['odom']['position']), flush=True) + + if mode == 'board': + res, _ = cli.request('click', u=args.u, v=args.v, + radius=args.radius, fit_plane=not args.no_fit) + p_map = res.get('p_map') + print('\n' + result_text(res, p_map, args.u, args.v) + '\n', flush=True) + if not (res.get('found') and p_map is not None): + return + else: + hc, cblob = cli.request('cloud') + xyz = decode_cloud(hc, cblob) + print('cloud: %d 点 (%.0f KB -> %.0f KB)' + % (xyz.shape[0], hc.get('raw_bytes', 0) / 1024.0, + hc.get('zlib_bytes', 0) / 1024.0), flush=True) + + idx = target_math.TargetIndex(xyz) + res, p_map = compute_target(xyz, h.get('odom'), args.u, args.v, + args.radius, not args.no_fit, index=idx) + print('\n' + result_text(res, p_map, args.u, args.v) + '\n', flush=True) + if not (res['found'] and p_map is not None): + return + + r, _ = cli.request('set_target', u=args.u, v=args.v, + p_cam=[float(x) for x in res['p_cam']], + p_lidar=[float(x) for x in res['p_lidar']], + p_map=[float(x) for x in p_map]) + print('set_target: ok=%s target=%s' % (r.get('ok'), r.get('target')), flush=True) + + if args.watch: + n = 0 + while args.watch_count is None or n < args.watch_count: + st, _ = cli.request('status') + print_status(st) + n += 1 + if args.watch_count is None or n < args.watch_count: + time.sleep(args.poll_ms / 1000.0) + + +# --------------------------------------------------------------------- 入口 +def parse_args(): + ap = argparse.ArgumentParser(description='会话式精度测试客户端') + ap.add_argument('--board', required=True, help='板子 IP，例如 192.168.43.50') + ap.add_argument('--port', type=int, default=5555) + ap.add_argument('--radius', type=float, default=10.0, help='候选像素半径') + ap.add_argument('--compute', choices=['board', 'pc'], default='board', + help='board=点云不出板（默认）；pc=拉点云到电脑算') + ap.add_argument('--no-fit', action='store_true', help='关闭平面拟合') + ap.add_argument('--no-gui', action='store_true') + ap.add_argument('--u', type=float, default=None, help='标定系像素 u（配合 --no-gui）') + ap.add_argument('--v', type=float, default=None, help='标定系像素 v（配合 --no-gui）') + ap.add_argument('--watch', action='store_true', help='点选后持续打印 Δ') + ap.add_argument('--watch-count', type=int, default=None) + ap.add_argument('--poll-ms', type=int, default=200) + ap.add_argument('--timeout-ms', type=int, default=8000) + ap.add_argument('--log', default='session_log.csv') + ap.add_argument('--no-log', action='store_true') + return ap.parse_args() + + +def main(): + args = parse_args() + cli = BoardClient(args.board, args.port, args.timeout_ms) + try: + if args.no_gui: + if args.u is None or args.v is None: + raise SystemExit('--no-gui 需要同时给 --u 和 --v（标定系像素）') + run_cli(cli, args) + else: + root = tk.Tk() + App(root, cli, args) + try: + root.mainloop() + finally: + cli.close() + finally: + cli.close() + + +if __name__ == '__main__': + main() diff --git a/client/target_math.py b/client/target_math.py new file mode 100644 index 0000000..cf9e0f6 --- /dev/null +++ b/client/target_math.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python3 +""" +target_math.py — 纯 numpy 的"像素 -> 三维目标"定位核心（不依赖 ROS，方便离线测试）。 + +输入：一帧点云（雷达系）+ 查询像素 +输出：目标在相机系/雷达系下的坐标、距离、置信度、是否有多个深度层等 +""" +import numpy as np + +import calib + + +def _fit_plane_ray(P, ray, max_residual=0.05): + """PCA 拟合平面，再让射线与平面求交。返回 (点, 残差)。""" + if P.shape[0] < 6: + return None, None + c = P.mean(axis=0) + _, _, vt = np.linalg.svd(P - c) + n = vt[2] + denom = float(n @ ray) + if abs(denom) < 1e-6: + return None, None + lam = float(n @ c) / denom + if lam <= 0: + return None, None + p = lam * ray + residual = float(np.sqrt(np.mean(((P - c) @ n) ** 2))) + if residual > max_residual: + return None, residual + return p, residual + + +def locate(xyz, query_u, query_v, conf=None, radius=10.0, depth_band=0.6, + fit_plane=True, max_residual=0.05, min_conf=None, min_z=0.05): + """在点云里找查询像素方向上最近的可见点。 + + 参数 + xyz : (N,3) 雷达系点云 + query_u/v : 查询像素 + conf : (N,) 置信度，可选 + radius : 候选像素半径 + 返回 dict（未找到时 found=False） + """ + xyz = np.asarray(xyz, dtype=np.float64) + good = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6) + if conf is not None and min_conf is not None: + good &= np.asarray(conf) >= min_conf + idx = np.where(good)[0] + pts = xyz[good] + + p_cam = calib.lidar_to_cam(pts) + u, v, ok = calib.project_cam_to_pixel(p_cam) + + dpx = np.hypot(u - query_u, v - query_v) + cand = ok & (dpx <= radius) & (p_cam[:, 2] > min_z) + + rng = np.linalg.norm(p_cam, axis=1) + res = { + 'found': False, 'n_valid': int(pts.shape[0]), 'n_cand': int(cand.sum()), + 'radius': float(radius), 'query_u': float(query_u), 'query_v': float(query_v), + } + if not cand.any(): + return res + + cand_idx = np.where(cand)[0] + order = cand_idx[np.argsort(rng[cand_idx])] + sel = int(order[0]) + + ray = calib.pixel_to_ray(query_u, query_v) + p_near = p_cam[sel] + + # 深度分层：候选距离跨度大 => 可能擦过边缘 + r_sorted = np.sort(rng[cand_idx]) + multi = bool(len(r_sorted) > 3 and (r_sorted[-1] - r_sorted[0]) > 2.0) + + p_final = p_near + plane_used = False + residual = None + if fit_plane: + band = p_cam[cand_idx][rng[cand_idx] <= rng[sel] + depth_band] + p_fit, residual = _fit_plane_ray(band, ray, max_residual) + if p_fit is not None: + p_final = p_fit + plane_used = True + + res.update({ + 'found': True, + 'p_cam': p_final, + 'range': float(np.linalg.norm(p_final)), + 'z': float(p_final[2]), + 'p_lidar': calib.cam_to_lidar(p_final)[0], + 'hit_u': float(u[sel]), + 'hit_v': float(v[sel]), + 'px_err': float(dpx[sel]), + 'lidar_index': int(idx[sel]), + 'plane_used': plane_used, + 'plane_residual': residual, + 'multi_layer': multi, + 'cand_ranges': rng[cand_idx], + 'confidence': float(conf[idx[sel]]) if conf is not None else None, + }) + return res + + +class TargetIndex: + """一帧点云的「像素格子 -> 点索引」倒排表。 + + 为什么需要： + locate() 每次点击都会把 N 个点全量投影一遍。N≈5 万时一次几毫秒到 + 几十毫秒，点一次无所谓；但同一帧反复点、或在板子上跑时，这部分是 + 重复劳动。这里在【每帧】只做一次全量投影 + 分桶，之后每次点击只 + 检查查询像素周围几个格子里的点，平面拟合也只在局部候选里做。 + + 用法： + idx = TargetIndex(xyz, conf=conf) # 每帧建一次 + res = idx.locate(u, v, radius=10) # 每次点击，返回结构和 locate 一样 + + 注意： + - 桶按点的像素坐标分，格子越大，查询时扫到的点越多；默认 32 px。 + - 只索引落在图像内的点；图像外的点本来也不会被点到。 + """ + + def __init__(self, xyz, conf=None, cell=32, min_z=0.05): + xyz = np.asarray(xyz, dtype=np.float64) + good = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6) + self.idx = np.where(good)[0] + self.xyz = xyz[good] + self.conf = None if conf is None else np.asarray(conf)[good] + + self.p_cam = calib.lidar_to_cam(self.xyz) + self.u, self.v, ok = calib.project_cam_to_pixel(self.p_cam) + self.rng = np.linalg.norm(self.p_cam, axis=1) + self.ok = ok & (self.p_cam[:, 2] > float(min_z)) + + self.cell = max(4, int(cell)) + self.nx = int(np.ceil(calib.IMAGE_WIDTH / float(self.cell))) + self.ny = int(np.ceil(calib.IMAGE_HEIGHT / float(self.cell))) + + sel = np.where(self.ok)[0] + if sel.size: + cx = np.clip((self.u[sel] // self.cell).astype(np.int64), + 0, self.nx - 1) + cy = np.clip((self.v[sel] // self.cell).astype(np.int64), + 0, self.ny - 1) + keys = cy * self.nx + cx + order = np.argsort(keys, kind='stable') + keys = keys[order] + sel = sel[order] + uniq, starts = np.unique(keys, return_index=True) + ends = np.append(starts[1:], keys.size) + self.buckets = {int(k): sel[s:e] + for k, s, e in zip(uniq, starts, ends)} + else: + self.buckets = {} + + def _gather(self, query_u, query_v, radius): + c = self.cell + x0 = max(0, int(np.floor((query_u - radius) / c))) + x1 = min(self.nx - 1, int(np.floor((query_u + radius) / c))) + y0 = max(0, int(np.floor((query_v - radius) / c))) + y1 = min(self.ny - 1, int(np.floor((query_v + radius) / c))) + + parts = [] + for cy in range(y0, y1 + 1): + base = cy * self.nx + for cx in range(x0, x1 + 1): + arr = self.buckets.get(base + cx) + if arr is not None and arr.size: + parts.append(arr) + if not parts: + return np.empty(0, dtype=np.int64) + return np.concatenate(parts) + + def locate(self, query_u, query_v, radius=10.0, depth_band=0.6, + fit_plane=True, max_residual=0.05, min_conf=None): + """局部查找，返回与 locate() 相同的字段。""" + query_u = float(query_u) + query_v = float(query_v) + + res = { + 'found': False, 'n_valid': int(self.idx.size), + 'n_cand': 0, 'radius': float(radius), + 'query_u': query_u, 'query_v': query_v, + 'indexed': True, + } + + cand = self._gather(query_u, query_v, radius) + if cand.size == 0: + return res + + dpx = np.hypot(self.u[cand] - query_u, self.v[cand] - query_v) + keep = dpx <= radius + if min_conf is not None and self.conf is not None: + keep &= self.conf[cand] >= min_conf + cand = cand[keep] + dpx = dpx[keep] + res['n_cand'] = int(cand.size) + if cand.size == 0: + return res + + # 距离最近的点 + order = np.argsort(self.rng[cand]) + cand = cand[order] + dpx = dpx[order] + sel = int(cand[0]) + + ray = calib.pixel_to_ray(query_u, query_v) + r_sorted = np.sort(self.rng[cand]) + multi = bool(r_sorted.size > 3 and (r_sorted[-1] - r_sorted[0]) > 2.0) + + p_final = self.p_cam[sel] + plane_used = False + residual = None + if fit_plane: + band = self.p_cam[cand[self.rng[cand] <= self.rng[sel] + depth_band]] + p_fit, residual = _fit_plane_ray(band, ray, max_residual) + if p_fit is not None: + p_final = p_fit + plane_used = True + + res.update({ + 'found': True, + 'p_cam': p_final, + 'p_lidar': calib.cam_to_lidar(p_final)[0], + 'hit_u': float(self.u[sel]), + 'hit_v': float(self.v[sel]), + 'px_err': float(dpx[0]), + 'lidar_index': int(self.idx[sel]), + 'plane_used': plane_used, + 'plane_residual': residual, + 'multi_layer': multi, + 'confidence': None if self.conf is None else float(self.conf[sel]), + }) + return res diff --git a/client/test_session.py b/client/test_session.py new file mode 100644 index 0000000..170617b --- /dev/null +++ b/client/test_session.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""test_session.py — 端到端测试：板子端算 / 电脑端算 / 会话隔离。""" +import time + +import numpy as np + +import _mock_server +import session_client as sc + + +def main(): + srv = _mock_server.MockServer(port=5599) + srv.start() + time.sleep(0.4) + try: + # ---------------- 会话 1：板子端算（点云不出板） + c1 = sc.BoardClient('127.0.0.1', 5599) + h, _ = c1.request('hello') + assert h['ok'] and h['board_can_compute'], h + + h, img = c1.request('capture') + assert h['ok'] and len(img) > 1000, h + + r, _ = c1.request('click', u=800.0, v=648.0, radius=10, fit_plane=True) + assert r['ok'] and r['found'], r + pm_board = np.asarray(r['p_map'], dtype=np.float64) + print('board-mode p_map =', np.round(pm_board, 4)) + # 合成点放在 3 m 处 + assert abs(np.linalg.norm(pm_board) - 3.0) < 0.06, pm_board + + st, _ = c1.request('status') + assert st['ok'] and st['has_target'] and st['delta'], st + print('board-mode status =', st['delta']) + assert abs(st['delta'][0] - (0.2507 - pm_board[0])) < 1e-6 + + # ---------------- 会话 2：电脑端算（拉点云 + 回传 p_map） + c2 = sc.BoardClient('127.0.0.1', 5599) + c2.request('hello') + h2, _ = c2.request('capture') + hc, cblob = c2.request('cloud') + xyz = sc.decode_cloud(hc, cblob) + idx = sc.target_math.TargetIndex(xyz) + res, pm_pc = sc.compute_target(xyz, h2['odom'], 800.0, 648.0, + 10.0, True, index=idx) + assert res['found'], res + print('pc-mode p_map =', np.round(pm_pc, 4)) + assert np.allclose(pm_pc, pm_board, atol=1e-6) + + r2, _ = c2.request('set_target', u=800, v=648, + p_cam=res['p_cam'].tolist(), + p_lidar=res['p_lidar'].tolist(), + p_map=pm_pc.tolist()) + assert r2['ok'], r2 + st2, _ = c2.request('status') + assert st2['has_target'], st2 + + # ---------------- 会话隔离：c1 的目标还在，c2 的 reset 不影响 c1 + st1, _ = c1.request('status') + assert st1['has_target'], st1 + c2.request('reset') + st2b, _ = c2.request('status') + assert not st2b['has_target'], st2b + st1b, _ = c1.request('status') + assert st1b['has_target'], st1b + print('session isolation: OK') + + # ---------------- 同一帧覆盖点选（索引复用） + t0 = time.perf_counter() + for du in (-8, -4, 0, 4, 8): + rr, _ = c1.request('click', u=800.0 + du, v=648.0, radius=10, fit_plane=True) + assert rr['ok'] + print('5 clicks on same frame: %.1f ms total' % ((time.perf_counter() - t0) * 1000)) + + c1.close() + c2.close() + print('PASS') + finally: + srv.stop() + + +if __name__ == '__main__': + main() diff --git a/client/wire.py b/client/wire.py new file mode 100644 index 0000000..9228c3b --- /dev/null +++ b/client/wire.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +wire.py — 会话消息的打包 / 解包。 + +pynng 一条消息只能是单块二进制，所以这里做一层很简单的外壳： + b"LS01" | u32 头长度 | JSON 头（UTF-8） | 二进制体 + +每个头里都带 session： + { + "id": "<一次运行一个 uuid>", + "client_ip": "192.168.43.23", # 电脑自己的地址 + "board_ip": "192.168.43.50", # 板子的地址 + "started_ns": 1712345678901234567, + "note": "..." + } + +这样每一条请求和应答都能看出属于哪个会话、从哪台机器来。 +""" +import json +import struct +import time +import uuid + +MAGIC = b"LS01" +_LEN = struct.Struct(" bytes。""" + h = json.dumps(header, separators=(",", ":"), ensure_ascii=False).encode("utf-8") + return MAGIC + _LEN.pack(len(h)) + h + bytes(blob) + + +def unpack(data): + """bytes -> (dict, 二进制体)。""" + if len(data) < 8 or data[:4] != MAGIC: + raise ValueError("bad frame magic") + n = _LEN.unpack_from(data, 4)[0] + if len(data) < 8 + n: + raise ValueError("truncated header") + header = json.loads(data[8:8 + n].decode("utf-8")) + return header, data[8 + n:] + + +def session_id(header): + return (header.get("session") or {}).get("id") + + +def check_session(header, session): + """应答必须属于当前会话，防止串台。""" + got = session_id(header) + if got != session["id"]: + raise RuntimeError("session mismatch: got %r, want %r" % (got, session["id"])) diff --git a/server/calib.py b/server/calib.py new file mode 100644 index 0000000..089e200 --- /dev/null +++ b/server/calib.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +""" +calib.py — Odin1 的相机/雷达标定参数与 FishPoly 鱼眼相机模型。 + +公式来源：odin_ros_driver/src/pointcloud_depth_converter.cpp + createDistortionMaps()（厂商逐像素实现），这里改写成向量化的连续函数。 + +坐标约定（已由驱动源码确认）： + Tcl_0 把【雷达系】的点变到【相机系】：p_cam = R · p_lidar + t + （证据：驱动里 Kcl_ = K * Tcl，然后 transformPointCloud(cloud, cloud_in_cam, Kcl_)） + +相机模型（FishPoly）： + 归一化针孔坐标 (x, y) -> r = sqrt(x²+y²) -> θ = atan(r) + θ_d = θ + k2θ² + k3θ³ + k4θ⁴ + k5θ⁵ + k6θ⁶ + k7θ⁷ + 畸变后的归一化坐标 = (x/r, y/r) * θ_d + 像素 = (A11·x_d + A12·y_d + u0, A22·y_d + v0) + +注意：这里所有"像素"都指**原始（未去畸变）图像**的像素，也就是 /odin1/image。 +""" +import numpy as np + +# ---------------- calib.yaml（设备 #O1-N090100083） ---------------- +A11 = 730.51652555414909 +A12 = -0.68917984297115186 +A22 = 730.30384140813805 +U0 = 785.10214573595919 +V0 = 670.33088344368650 + +K2 = -8.2510665959742261e-03 +K3 = 1.6324575756549702e-02 +K4 = -5.6837005957061821e-02 +K5 = 4.9991959834060670e-02 +K6 = -2.4463726125325536e-02 +K7 = 3.8528646040351232e-03 + +IMAGE_WIDTH = 1600 +IMAGE_HEIGHT = 1296 +MAX_INCIDENT_DEG = 120.0 + +# Tcl_0：雷达 -> 相机 +TCL = np.array([ + [-0.00177, -0.99998, 0.00586, 0.03809], + [ 0.00744, -0.00588, -0.99996, -0.00189], + [ 0.99997, -0.00173, 0.00745, -0.01966], + [ 0.0, 0.0, 0.0, 1.0 ], +], dtype=np.float64) +R_CAM_LIDAR = TCL[:3, :3] +T_CAM_LIDAR = TCL[:3, 3] + +# 安全上限：θ 接近 90° 时 tan(θ) 发散。 +# 这颗镜头水平半视角约 63°，图像四角可到 ~87°，所以上限放到 89°。 +_THETA_MAX = np.deg2rad(89.0) + + +def theta_d(theta): + """把入射角 θ 映射成畸变后的归一化半径。""" + th = np.asarray(theta, dtype=np.float64) + return (th + K2 * th**2 + K3 * th**3 + K4 * th**4 + + K5 * th**5 + K6 * th**6 + K7 * th**7) + + +def dtheta_d(theta): + """θ_d 对 θ 的导数（牛顿迭代用）。""" + th = np.asarray(theta, dtype=np.float64) + return (1.0 + 2 * K2 * th + 3 * K3 * th**2 + 4 * K4 * th**3 + + 5 * K5 * th**4 + 6 * K6 * th**5 + 7 * K7 * th**6) + + +def solve_theta(rho_d, iters=30): + """反解：给定畸变归一化半径 rho_d，求入射角 θ。""" + rho_d = np.asarray(rho_d, dtype=np.float64) + th = np.minimum(rho_d, _THETA_MAX).astype(np.float64) + for _ in range(iters): + f = theta_d(th) - rho_d + fp = dtheta_d(th) + fp = np.where(np.abs(fp) < 1e-12, 1e-12, fp) + step = f / fp + th = np.clip(th - step, 0.0, _THETA_MAX) + if np.all(np.abs(step) < 1e-12): + break + return th + + +def pixel_to_ray(u, v): + """原始(畸变)图像像素 -> 相机系下的单位方向向量。 + + 标量输入返回 (3,)，数组输入返回 (N,3)。 + """ + u = np.asarray(u, dtype=np.float64) + v = np.asarray(v, dtype=np.float64) + yd = (v - V0) / A22 + xd = (u - U0 - A12 * yd) / A11 + + rho = np.hypot(xd, yd) + small = rho < 1e-9 + rho_safe = np.where(small, 1.0, rho) + + theta = solve_theta(rho) + r = np.tan(theta) + + x = xd * (r / rho_safe) + y = yd * (r / rho_safe) + d = np.stack([x, y, np.ones_like(x)], axis=-1) + d = d / np.linalg.norm(d, axis=-1, keepdims=True) + return d + + +def project_cam_to_pixel(p_cam, margin=0.0): + """相机系点 (N,3) -> 原始图像像素 (u, v, valid)。""" + p = np.atleast_2d(np.asarray(p_cam, dtype=np.float64)) + Z = p[:, 2] + valid = np.isfinite(p).all(axis=1) & (Z > 1e-6) + Zs = np.where(valid, Z, 1.0) + + x = p[:, 0] / Zs + y = p[:, 1] / Zs + r = np.hypot(x, y) + theta = np.arctan(r) + thd = theta_d(theta) + + s = np.where(r > 1e-9, thd / np.where(r > 1e-9, r, 1.0), 1.0) + xd = x * s + yd = y * s + + u = A11 * xd + A12 * yd + U0 + v = A22 * yd + V0 + valid &= np.isfinite(u) & np.isfinite(v) + valid &= (u >= -margin) & (u <= IMAGE_WIDTH - 1 + margin) + valid &= (v >= -margin) & (v <= IMAGE_HEIGHT - 1 + margin) + return u, v, valid + + +def lidar_to_cam(pts): + """雷达系点 (N,3) -> 相机系点 (N,3)。""" + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + return pts @ R_CAM_LIDAR.T + T_CAM_LIDAR + + +def cam_to_lidar(pts): + """相机系点 (N,3) -> 雷达系点 (N,3)。""" + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + return (pts - T_CAM_LIDAR) @ R_CAM_LIDAR + + +def cam_to_base_link(pts, cam_xyz=(0.0, 0.0, 0.0), rpy_deg=(0.0, 0.0, 0.0)): + """相机系 -> 车体系。 + + 坐标约定： + 相机系：x 右, y 下, z 前 + 车体系：x 前, y 左, z 上（ROS REP-103, base_link） + + 参数： + cam_xyz : 相机光心在车体系下的位置 (m)，例如 (0.1, 0.0, 0.3) + rpy_deg : 相机相对车体的姿态 (roll, pitch, yaw)，单位度 + pitch > 0 表示镜头向下俯（常见安装） + """ + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + R = _base_from_cam_rotation(rpy_deg) + return pts @ R.T + np.asarray(cam_xyz, dtype=np.float64) + + +def base_link_to_cam(pts, cam_xyz=(0.0, 0.0, 0.0), rpy_deg=(0.0, 0.0, 0.0)): + """车体系 -> 相机系（cam_to_base_link 的逆）。""" + pts = np.atleast_2d(np.asarray(pts, dtype=np.float64)) + R = _base_from_cam_rotation(rpy_deg) + return (pts - np.asarray(cam_xyz, dtype=np.float64)) @ R + + +def _base_from_cam_rotation(rpy_deg=(0.0, 0.0, 0.0)): + """构造 R_base_cam。零姿态时：相机 z(前)->车体 x(前)，x(右)->车体 -y，y(下)->车体 -z。""" + roll, pitch, yaw = np.deg2rad(rpy_deg) + Rx = np.array([[1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)]]) + Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)]]) + Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1]]) + R0 = np.array([[0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0]]) + return Rz @ Ry @ Rx @ R0 + + +if __name__ == '__main__': + # 往返一致性自测：像素 -> 射线 -> 投回像素 + print('Tcl determinant (should be +1):', np.linalg.det(R_CAM_LIDAR)) + print('camera position in lidar frame (m):', cam_to_lidar(np.zeros((1, 3)))[0]) + print() + for u, v in [(800, 648), (100, 100), (1500, 1200), (400, 900), (1599, 0)]: + d = pixel_to_ray(u, v) + p = d * 3.0 + u2, v2, ok = project_cam_to_pixel(p) + print('pixel (%4d,%4d) -> ray %s -> back (%8.2f,%8.2f) valid=%s err=%.2e' + % (u, v, np.round(d, 4), u2[0], v2[0], ok[0], + np.hypot(u2[0] - u, v2[0] - v))) diff --git a/server/requirements.txt b/server/requirements.txt new file mode 100644 index 0000000..5851103 --- /dev/null +++ b/server/requirements.txt @@ -0,0 +1,12 @@ +# 板子端（Ubuntu 24.04 + ROS2 Jazzy） +# +# ROS2 本体用 apt 安装，不要写进 pip requirements： +# sudo apt install ros-jazzy-rclpy ros-jazzy-sensor-msgs \ +# ros-jazzy-nav-msgs ros-jazzy-message-filters +# +pynng>=0.9.0 +numpy>=1.26 + +# 仅当使用 --no-compressed（原始图需要编码）时，二选一： +# opencv-python>=4.8 +# Pillow>=10.0 diff --git a/server/session_server_node.py b/server/session_server_node.py new file mode 100644 index 0000000..db27ffe --- /dev/null +++ b/server/session_server_node.py @@ -0,0 +1,611 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +session_server_node.py — 板子端：会话式「抓帧 / 回传图像 / 回传点云 / 误差跟踪」 + +一次运行 = 一个会话。电脑端每一条请求、板子每一条应答都带 session +（id / client_ip / board_ip / started_ns），会话之间状态隔离。 + +pynng REQ/REP 协议（默认 tcp://0.0.0.0:5555）： + + hello 握手；返回标定分辨率、是否支持板子端计算 + capture 等一帧同步的（图像 + 点云 + 位姿），返回图像字节； + 本会话记住这一帧（点云、位姿），并清掉旧目标 + click 默认：电脑只发 (u, v)，板子用本地点云算 + p_cam / p_lidar / p_map，并把它记成目标（点云不出板） + cloud 可选（--compute pc）：返回 capture 那一帧的点云 + set_target 可选（--compute pc）：电脑算好的 p_map 回传 + status 返回当前 pos / yaw / target / Δ（只给坐标，不给距离） + reset 清除本会话的目标 + +投影优化： + click 第一次会建一个「像素格子 -> 点索引」倒排表（每帧一次）， + 之后同一帧的点击只查周围几个格子，不再全量投影，平面拟合也只在 + 局部候选里做。格子大小 --index-cell（默认 32 px）。 + +用法（板子）： + source /opt/ros/jazzy/setup.bash + python3 session_server_node.py --port 5555 + +常用参数： + --image-topic /odin1/image/compressed （默认走压缩图，直接转发，不用编码） + --no-compressed （强制用原始 /odin1/image，需要 cv2 或 Pillow） + --cloud-topic /odin1/cloud_raw + --odom-topic /odin1/odometry +""" +import argparse +import threading +import time +import zlib +from collections import deque + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import (QoSProfile, ReliabilityPolicy, HistoryPolicy, + qos_profile_sensor_data) +from sensor_msgs.msg import Image, CompressedImage, PointCloud2 +from nav_msgs.msg import Odometry +from message_filters import Subscriber, ApproximateTimeSynchronizer + +import pynng + +import wire + +try: + import calib + CALIB_W = int(calib.IMAGE_WIDTH) + CALIB_H = int(calib.IMAGE_HEIGHT) +except Exception: # 没放 calib.py 也能跑，只是分辨率用默认值 + CALIB_W, CALIB_H = 1600, 1296 + +# 板子端本地算 p_cam/p_lidar/p_map（默认模式）。没有 target_math.py 时 +# 自动降级为"电脑端算 + 点云回传"。 +try: + import target_math + _HAS_TARGET_MATH = True +except Exception: + target_math = None + _HAS_TARGET_MATH = False + +# sensor_msgs/PointField.datatype -> numpy dtype +_PF = {1: np.int8, 2: np.uint8, 3: np.int16, 4: np.uint16, + 5: np.int32, 6: np.uint32, 7: np.float32, 8: np.float64} +_CH = {'bgr8': 3, 'rgb8': 3, 'bgra8': 4, 'rgba8': 4, 'mono8': 1} + + +def stamp_ns(h): + return int(h.stamp.sec) * 1_000_000_000 + int(h.stamp.nanosec) + + +def unpack_cloud(msg): + """按字段名/偏移解析 PointCloud2（兼容 padding 与非对齐字段）。""" + names, formats, offsets = [], [], [] + for f in msg.fields: + if f.datatype not in _PF: + continue + names.append(f.name) + formats.append(_PF[f.datatype]) + offsets.append(f.offset) + dt = np.dtype({'names': names, 'formats': formats, + 'offsets': offsets, 'itemsize': msg.point_step}) + arr = np.frombuffer(msg.data, dtype=dt, count=msg.width * msg.height) + return arr, names + + +def image_to_array(msg): + ch = _CH.get(msg.encoding) + if ch is None: + return None + row = msg.step if msg.step else msg.width * ch + buf = np.frombuffer(msg.data, np.uint8).reshape(msg.height, row) + img = buf[:, :msg.width * ch].reshape(msg.height, msg.width, ch) + return img.copy() + + +def quat_to_yaw(x, y, z, w): + return float(np.arctan2(2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z))) + + +def quat_to_rot(q): + x, y, z, w = [float(v) for v in q] + return np.array([ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ], dtype=np.float64) + + +class SessionServer(Node): + def __init__(self, args): + super().__init__('session_server') + self.args = args + self._stop = False + self._rep = None + + self._lock = threading.Lock() + self._cv = threading.Condition(self._lock) + self._latest = None + self._odom_hist = deque(maxlen=400) + self._sessions = {} + + self._setup_subs() + + self.get_logger().info( + '会话服务: image=%s cloud=%s odom=%s' + % (args.image_topic, args.cloud_topic, args.odom_topic)) + self.get_logger().info( + '标定分辨率 %dx%d，等待 %s 的第一帧…' % (CALIB_W, CALIB_H, args.image_topic)) + + # ------------------------------------------------------------------ 订阅 + def _setup_subs(self): + img_type = CompressedImage if self.args.image_topic.rstrip('/').endswith('/compressed') else Image + a = Subscriber(self, img_type, self.args.image_topic, + qos_profile=qos_profile_sensor_data) + b = Subscriber(self, PointCloud2, self.args.cloud_topic, + qos_profile=qos_profile_sensor_data) + self.sync = ApproximateTimeSynchronizer([a, b], + queue_size=self.args.queue, + slop=self.args.slop) + self.sync.registerCallback(self.on_pair) + + # 里程计：best-effort 订阅既能接 reliable 也能接 best-effort 的发布者 + odom_qos = QoSProfile(depth=100, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST) + self.create_subscription(Odometry, self.args.odom_topic, + self.on_odom, odom_qos) + + def on_odom(self, msg): + p = msg.pose.pose.position + q = msg.pose.pose.orientation + rec = { + 'stamp_ns': stamp_ns(msg.header), + 'recv_mono': time.monotonic(), + 'pos': (float(p.x), float(p.y), float(p.z)), + 'quat': (float(q.x), float(q.y), float(q.z), float(q.w)), + 'frame_id': msg.header.frame_id, + 'child_frame_id': msg.child_frame_id, + } + with self._lock: + self._odom_hist.append(rec) + + def on_pair(self, img_msg, pc_msg): + """图像 + 点云时间对齐后，把这一帧放进 latest。""" + arr, names = unpack_cloud(pc_msg) + if not all(k in names for k in ('x', 'y', 'z')): + self.get_logger().warn('点云没有 x/y/z 字段，实际：%s' % names) + return + + xyz = np.stack([arr['x'], arr['y'], arr['z']], axis=-1).astype(np.float32) + conf = np.ascontiguousarray(arr['confidence']) if 'confidence' in names else None + cloud_stamp = stamp_ns(pc_msg.header) + img_stamp = stamp_ns(img_msg.header) + + if isinstance(img_msg, CompressedImage): + frame = { + 'kind': 'compressed', + 'blob': bytes(img_msg.data), + 'format': str(img_msg.format or 'jpeg'), + 'width': None, + 'height': None, + } + else: + img = image_to_array(img_msg) + if img is None: + self.get_logger().warn('不支持的图像编码：%s' % img_msg.encoding) + return + frame = { + 'kind': 'raw', + 'img': img, + 'format': None, + 'width': int(img_msg.width), + 'height': int(img_msg.height), + 'encoding': img_msg.encoding, + } + + frame.update({ + 'xyz': xyz, + 'conf': conf, + 'cloud_stamp_ns': cloud_stamp, + 'img_stamp_ns': img_stamp, + 'cloud_frame_id': pc_msg.header.frame_id, + 'recv_mono': time.monotonic(), + }) + + with self._cv: + self._latest = frame + self._cv.notify_all() + + self.get_logger().debug( + '同步帧: 图像 %s, 点云 %d 点, 时间差 %.2f ms' + % (frame['kind'], xyz.shape[0], (cloud_stamp - img_stamp) / 1e6)) + + # ------------------------------------------------------------------ 帧工具 + def wait_for_frame(self, after_mono, timeout): + """等一帧 recv 时间 >= after_mono 的帧；超时返回当前最新帧。""" + deadline = time.monotonic() + timeout + with self._cv: + while True: + frame = self._latest + if frame is not None and frame['recv_mono'] >= after_mono: + return dict(frame) + left = deadline - time.monotonic() + if left <= 0: + return dict(frame) if frame is not None else None + self._cv.wait(min(left, 0.2)) + + def _nearest_odom_locked(self, stamp_ns_value): + if not self._odom_hist: + return None + return min(self._odom_hist, key=lambda r: abs(r['stamp_ns'] - stamp_ns_value)) + + def frame_odom(self, frame): + with self._lock: + od = self._nearest_odom_locked(frame['cloud_stamp_ns']) + if od is None: + return None + return { + 'stamp_ns': int(od['stamp_ns']), + 'position': [float(v) for v in od['pos']], + 'orientation_xyzw': [float(v) for v in od['quat']], + 'frame_id': od['frame_id'], + 'child_frame_id': od['child_frame_id'], + 'delta_ms': (od['stamp_ns'] - frame['cloud_stamp_ns']) / 1e6, + } + + def encode_image(self, frame): + """返回 (bytes, format)。压缩图直接转发；原图才编码。""" + if frame.get('kind') == 'compressed': + return frame['blob'], frame.get('format') or 'jpeg' + + img = frame['img'] + try: + import cv2 + ok, buf = cv2.imencode( + '.jpg', img, + [int(cv2.IMWRITE_JPEG_QUALITY), int(self.args.jpeg_quality)]) + if ok: + return buf.tobytes(), 'jpeg' + except Exception: + pass + + try: + import io + from PIL import Image as PILImage + enc = frame.get('encoding', 'bgr8') + rgb = img[..., ::-1] if enc.startswith('bgr') else img[..., :3] + b = io.BytesIO() + PILImage.fromarray(np.ascontiguousarray(rgb)).save( + b, format='JPEG', quality=int(self.args.jpeg_quality)) + return b.getvalue(), 'jpeg' + except Exception as exc: + raise RuntimeError('原图编码失败，需要 OpenCV 或 Pillow：%s' % exc) + + # ------------------------------------------------------------------ 会话 + def _prune(self): + now = time.monotonic() + for sid in list(self._sessions): + if now - self._sessions[sid]['last'] > self.args.session_ttl: + del self._sessions[sid] + + def handle_hello(self, hdr): + sid = hdr['session']['id'] + with self._lock: + self._sessions.setdefault(sid, { + 'session': hdr['session'], 'frame': None, + 'target': None, 'target_info': None, + 'last': time.monotonic(), + }) + self._sessions[sid]['last'] = time.monotonic() + return { + 'ok': True, 'type': 'hello', + 'node': self.get_name(), + 'calib_width': CALIB_W, 'calib_height': CALIB_H, + 'board_can_compute': bool(_HAS_TARGET_MATH), + 'compute_modes': (['board', 'pc'] if _HAS_TARGET_MATH else ['pc']), + 'topics': {'image': self.args.image_topic, + 'cloud': self.args.cloud_topic, + 'odom': self.args.odom_topic}, + } + + def handle_capture(self, hdr): + self._prune() + t_req = time.monotonic() + frame = self.wait_for_frame(t_req - self.args.max_frame_age, + self.args.frame_timeout) + if frame is None: + return {'ok': False, 'type': 'frame', + 'error': '还没有收到同步的图像+点云'}, b'' + + blob, fmt = self.encode_image(frame) + sid = hdr['session']['id'] + with self._lock: + self._sessions[sid] = { + 'session': hdr['session'], 'frame': frame, + 'target': None, 'target_info': None, + 'last': time.monotonic(), + } + + rep = { + 'ok': True, 'type': 'frame', + 'image_format': fmt, + 'image_bytes': len(blob), + 'image_width': frame.get('width'), + 'image_height': frame.get('height'), + 'calib_width': CALIB_W, 'calib_height': CALIB_H, + 'image_stamp_ns': int(frame['img_stamp_ns']), + 'cloud_stamp_ns': int(frame['cloud_stamp_ns']), + 'stamp_delta_ms': (frame['cloud_stamp_ns'] - frame['img_stamp_ns']) / 1e6, + 'n_points': int(frame['xyz'].shape[0]), + 'board_can_compute': bool(_HAS_TARGET_MATH), + 'odom': self.frame_odom(frame), + } + return rep, blob + + def handle_click(self, hdr): + """板子端算：像素 -> p_cam / p_lidar / p_map，并把它记成目标。 + + 这是默认模式：点云不出板，电脑只发 (u, v)。 + """ + if not _HAS_TARGET_MATH: + return {'ok': False, 'type': 'target', + 'error': '板子没装 target_math.py，请电脑端用 --compute pc'}, b'' + + sid = hdr['session']['id'] + with self._lock: + s = self._sessions.get(sid) + frame = None if s is None else s.get('frame') + if frame is None: + return {'ok': False, 'type': 'target', 'error': '先 capture'}, b'' + + u = float(hdr['u']) + v = float(hdr['v']) + radius = float(hdr.get('radius', self.args.radius)) + fit = bool(hdr.get('fit_plane', True)) + + # 每帧只建一次索引；同一帧后续点击只查局部格子 + with self._lock: + index = None if s is None else s.get('index') + if index is None: + index = target_math.TargetIndex(frame['xyz'], conf=frame.get('conf'), + cell=self.args.index_cell) + with self._lock: + if s is not None: + s['index'] = index + + res = index.locate(u, v, radius=radius, fit_plane=fit) + out = { + 'ok': True, 'type': 'target', + 'found': bool(res['found']), + 'u': u, 'v': v, + 'n_valid': int(res['n_valid']), + 'n_cand': int(res['n_cand']), + 'plane_used': bool(res['plane_used']), + 'multi_layer': bool(res['multi_layer']), + } + if not res['found']: + return out, b'' + + out.update({ + 'p_cam': [float(x) for x in res['p_cam']], + 'p_lidar': [float(x) for x in res['p_lidar']], + 'hit_u': float(res['hit_u']), + 'hit_v': float(res['hit_v']), + 'px_err': float(res['px_err']), + 'confidence': (None if res.get('confidence') is None + else float(res['confidence'])), + }) + + od = self.frame_odom(frame) + if od is None: + out['p_map'] = None + out['error'] = '本帧没有里程计位姿' + return out, b'' + + R = quat_to_rot(od['orientation_xyzw']) + t = np.asarray(od['position'], dtype=np.float64) + pm = R @ np.asarray(res['p_lidar'], dtype=np.float64) + t + + out['p_map'] = [float(x) for x in pm] + out['odom'] = od + with self._lock: + s['target'] = pm + s['target_info'] = out + s['last'] = time.monotonic() + return out, b'' + + def handle_cloud(self, hdr): + sid = hdr['session']['id'] + with self._lock: + s = self._sessions.get(sid) + frame = None if s is None else s.get('frame') + if frame is None: + return {'ok': False, 'type': 'cloud', 'error': '先 capture'}, b'' + + xyz = np.asarray(frame['xyz'], dtype=' ok=%s' + % (sid[:8], typ, sess.get('client_ip', '?'), reply.get('ok'))) + except Exception as exc: + try: + self._rep.send(wire.pack({ + 'ok': False, 'type': hdr.get('type', 'error'), + 'error': str(exc), + 'session': hdr.get('session') or {}, + 'server_ns': time.time_ns(), + })) + except Exception: + pass + self.get_logger().error('处理请求失败: %s' % exc) + + def stop(self): + self._stop = True + if self._rep is not None: + try: + self._rep.close() + except Exception: + pass + + +def parse_args(): + ap = argparse.ArgumentParser(description='会话式抓帧/定位/误差服务') + ap.add_argument('--port', type=int, default=5555) + ap.add_argument('--image-topic', default='/odin1/image/compressed') + ap.add_argument('--cloud-topic', default='/odin1/cloud_raw') + ap.add_argument('--odom-topic', default='/odin1/odometry') + ap.add_argument('--no-compressed', action='store_true', + help='强制用原始 /odin1/image（需要 cv2 或 Pillow）') + ap.add_argument('--slop', type=float, default=0.08, help='图像/点云允许时间差(秒)') + ap.add_argument('--queue', type=int, default=20) + ap.add_argument('--max-frame-age', type=float, default=0.35, + help='抓帧时允许复用多旧的帧(秒)') + ap.add_argument('--frame-timeout', type=float, default=2.0, + help='等新帧的最长时间(秒)') + ap.add_argument('--jpeg-quality', type=int, default=85) + ap.add_argument('--radius', type=float, default=10.0, + help='click 请求没带半径时的默认候选像素半径') + ap.add_argument('--index-cell', type=int, default=32, + help='像素格子大小(px)，用于每帧的投影索引') + ap.add_argument('--odom-timeout', type=float, default=1.0, + help='里程计超过这个时间没更新就标记 invalid(秒)') + ap.add_argument('--session-ttl', type=float, default=600.0) + args = ap.parse_args() + if args.no_compressed: + args.image_topic = '/odin1/image' + return args + + +def main(): + args = parse_args() + rclpy.init() + node = SessionServer(args) + th = threading.Thread(target=node.serve, daemon=True) + th.start() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.stop() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/server/target_math.py b/server/target_math.py new file mode 100644 index 0000000..cf9e0f6 --- /dev/null +++ b/server/target_math.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python3 +""" +target_math.py — 纯 numpy 的"像素 -> 三维目标"定位核心（不依赖 ROS，方便离线测试）。 + +输入：一帧点云（雷达系）+ 查询像素 +输出：目标在相机系/雷达系下的坐标、距离、置信度、是否有多个深度层等 +""" +import numpy as np + +import calib + + +def _fit_plane_ray(P, ray, max_residual=0.05): + """PCA 拟合平面，再让射线与平面求交。返回 (点, 残差)。""" + if P.shape[0] < 6: + return None, None + c = P.mean(axis=0) + _, _, vt = np.linalg.svd(P - c) + n = vt[2] + denom = float(n @ ray) + if abs(denom) < 1e-6: + return None, None + lam = float(n @ c) / denom + if lam <= 0: + return None, None + p = lam * ray + residual = float(np.sqrt(np.mean(((P - c) @ n) ** 2))) + if residual > max_residual: + return None, residual + return p, residual + + +def locate(xyz, query_u, query_v, conf=None, radius=10.0, depth_band=0.6, + fit_plane=True, max_residual=0.05, min_conf=None, min_z=0.05): + """在点云里找查询像素方向上最近的可见点。 + + 参数 + xyz : (N,3) 雷达系点云 + query_u/v : 查询像素 + conf : (N,) 置信度，可选 + radius : 候选像素半径 + 返回 dict（未找到时 found=False） + """ + xyz = np.asarray(xyz, dtype=np.float64) + good = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6) + if conf is not None and min_conf is not None: + good &= np.asarray(conf) >= min_conf + idx = np.where(good)[0] + pts = xyz[good] + + p_cam = calib.lidar_to_cam(pts) + u, v, ok = calib.project_cam_to_pixel(p_cam) + + dpx = np.hypot(u - query_u, v - query_v) + cand = ok & (dpx <= radius) & (p_cam[:, 2] > min_z) + + rng = np.linalg.norm(p_cam, axis=1) + res = { + 'found': False, 'n_valid': int(pts.shape[0]), 'n_cand': int(cand.sum()), + 'radius': float(radius), 'query_u': float(query_u), 'query_v': float(query_v), + } + if not cand.any(): + return res + + cand_idx = np.where(cand)[0] + order = cand_idx[np.argsort(rng[cand_idx])] + sel = int(order[0]) + + ray = calib.pixel_to_ray(query_u, query_v) + p_near = p_cam[sel] + + # 深度分层：候选距离跨度大 => 可能擦过边缘 + r_sorted = np.sort(rng[cand_idx]) + multi = bool(len(r_sorted) > 3 and (r_sorted[-1] - r_sorted[0]) > 2.0) + + p_final = p_near + plane_used = False + residual = None + if fit_plane: + band = p_cam[cand_idx][rng[cand_idx] <= rng[sel] + depth_band] + p_fit, residual = _fit_plane_ray(band, ray, max_residual) + if p_fit is not None: + p_final = p_fit + plane_used = True + + res.update({ + 'found': True, + 'p_cam': p_final, + 'range': float(np.linalg.norm(p_final)), + 'z': float(p_final[2]), + 'p_lidar': calib.cam_to_lidar(p_final)[0], + 'hit_u': float(u[sel]), + 'hit_v': float(v[sel]), + 'px_err': float(dpx[sel]), + 'lidar_index': int(idx[sel]), + 'plane_used': plane_used, + 'plane_residual': residual, + 'multi_layer': multi, + 'cand_ranges': rng[cand_idx], + 'confidence': float(conf[idx[sel]]) if conf is not None else None, + }) + return res + + +class TargetIndex: + """一帧点云的「像素格子 -> 点索引」倒排表。 + + 为什么需要： + locate() 每次点击都会把 N 个点全量投影一遍。N≈5 万时一次几毫秒到 + 几十毫秒，点一次无所谓；但同一帧反复点、或在板子上跑时，这部分是 + 重复劳动。这里在【每帧】只做一次全量投影 + 分桶，之后每次点击只 + 检查查询像素周围几个格子里的点，平面拟合也只在局部候选里做。 + + 用法： + idx = TargetIndex(xyz, conf=conf) # 每帧建一次 + res = idx.locate(u, v, radius=10) # 每次点击，返回结构和 locate 一样 + + 注意： + - 桶按点的像素坐标分，格子越大，查询时扫到的点越多；默认 32 px。 + - 只索引落在图像内的点；图像外的点本来也不会被点到。 + """ + + def __init__(self, xyz, conf=None, cell=32, min_z=0.05): + xyz = np.asarray(xyz, dtype=np.float64) + good = np.isfinite(xyz).all(axis=1) & (np.abs(xyz).sum(axis=1) > 1e-6) + self.idx = np.where(good)[0] + self.xyz = xyz[good] + self.conf = None if conf is None else np.asarray(conf)[good] + + self.p_cam = calib.lidar_to_cam(self.xyz) + self.u, self.v, ok = calib.project_cam_to_pixel(self.p_cam) + self.rng = np.linalg.norm(self.p_cam, axis=1) + self.ok = ok & (self.p_cam[:, 2] > float(min_z)) + + self.cell = max(4, int(cell)) + self.nx = int(np.ceil(calib.IMAGE_WIDTH / float(self.cell))) + self.ny = int(np.ceil(calib.IMAGE_HEIGHT / float(self.cell))) + + sel = np.where(self.ok)[0] + if sel.size: + cx = np.clip((self.u[sel] // self.cell).astype(np.int64), + 0, self.nx - 1) + cy = np.clip((self.v[sel] // self.cell).astype(np.int64), + 0, self.ny - 1) + keys = cy * self.nx + cx + order = np.argsort(keys, kind='stable') + keys = keys[order] + sel = sel[order] + uniq, starts = np.unique(keys, return_index=True) + ends = np.append(starts[1:], keys.size) + self.buckets = {int(k): sel[s:e] + for k, s, e in zip(uniq, starts, ends)} + else: + self.buckets = {} + + def _gather(self, query_u, query_v, radius): + c = self.cell + x0 = max(0, int(np.floor((query_u - radius) / c))) + x1 = min(self.nx - 1, int(np.floor((query_u + radius) / c))) + y0 = max(0, int(np.floor((query_v - radius) / c))) + y1 = min(self.ny - 1, int(np.floor((query_v + radius) / c))) + + parts = [] + for cy in range(y0, y1 + 1): + base = cy * self.nx + for cx in range(x0, x1 + 1): + arr = self.buckets.get(base + cx) + if arr is not None and arr.size: + parts.append(arr) + if not parts: + return np.empty(0, dtype=np.int64) + return np.concatenate(parts) + + def locate(self, query_u, query_v, radius=10.0, depth_band=0.6, + fit_plane=True, max_residual=0.05, min_conf=None): + """局部查找，返回与 locate() 相同的字段。""" + query_u = float(query_u) + query_v = float(query_v) + + res = { + 'found': False, 'n_valid': int(self.idx.size), + 'n_cand': 0, 'radius': float(radius), + 'query_u': query_u, 'query_v': query_v, + 'indexed': True, + } + + cand = self._gather(query_u, query_v, radius) + if cand.size == 0: + return res + + dpx = np.hypot(self.u[cand] - query_u, self.v[cand] - query_v) + keep = dpx <= radius + if min_conf is not None and self.conf is not None: + keep &= self.conf[cand] >= min_conf + cand = cand[keep] + dpx = dpx[keep] + res['n_cand'] = int(cand.size) + if cand.size == 0: + return res + + # 距离最近的点 + order = np.argsort(self.rng[cand]) + cand = cand[order] + dpx = dpx[order] + sel = int(cand[0]) + + ray = calib.pixel_to_ray(query_u, query_v) + r_sorted = np.sort(self.rng[cand]) + multi = bool(r_sorted.size > 3 and (r_sorted[-1] - r_sorted[0]) > 2.0) + + p_final = self.p_cam[sel] + plane_used = False + residual = None + if fit_plane: + band = self.p_cam[cand[self.rng[cand] <= self.rng[sel] + depth_band]] + p_fit, residual = _fit_plane_ray(band, ray, max_residual) + if p_fit is not None: + p_final = p_fit + plane_used = True + + res.update({ + 'found': True, + 'p_cam': p_final, + 'p_lidar': calib.cam_to_lidar(p_final)[0], + 'hit_u': float(self.u[sel]), + 'hit_v': float(self.v[sel]), + 'px_err': float(dpx[0]), + 'lidar_index': int(self.idx[sel]), + 'plane_used': plane_used, + 'plane_residual': residual, + 'multi_layer': multi, + 'confidence': None if self.conf is None else float(self.conf[sel]), + }) + return res diff --git a/server/wire.py b/server/wire.py new file mode 100644 index 0000000..9228c3b --- /dev/null +++ b/server/wire.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +wire.py — 会话消息的打包 / 解包。 + +pynng 一条消息只能是单块二进制，所以这里做一层很简单的外壳： + b"LS01" | u32 头长度 | JSON 头（UTF-8） | 二进制体 + +每个头里都带 session： + { + "id": "<一次运行一个 uuid>", + "client_ip": "192.168.43.23", # 电脑自己的地址 + "board_ip": "192.168.43.50", # 板子的地址 + "started_ns": 1712345678901234567, + "note": "..." + } + +这样每一条请求和应答都能看出属于哪个会话、从哪台机器来。 +""" +import json +import struct +import time +import uuid + +MAGIC = b"LS01" +_LEN = struct.Struct(" bytes。""" + h = json.dumps(header, separators=(",", ":"), ensure_ascii=False).encode("utf-8") + return MAGIC + _LEN.pack(len(h)) + h + bytes(blob) + + +def unpack(data): + """bytes -> (dict, 二进制体)。""" + if len(data) < 8 or data[:4] != MAGIC: + raise ValueError("bad frame magic") + n = _LEN.unpack_from(data, 4)[0] + if len(data) < 8 + n: + raise ValueError("truncated header") + header = json.loads(data[8:8 + n].decode("utf-8")) + return header, data[8 + n:] + + +def session_id(header): + return (header.get("session") or {}).get("id") + + +def check_session(header, session): + """应答必须属于当前会话，防止串台。""" + got = session_id(header) + if got != session["id"]: + raise RuntimeError("session mismatch: got %r, want %r" % (got, session["id"]))