initial commit
This commit is contained in:
@@ -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": "<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`。
|
||||
@@ -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`。
|
||||
@@ -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')
|
||||
@@ -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('<f4').tobytes(), 1)
|
||||
return {'ok': True, 'type': 'cloud', 'encoding': 'zlib_f32',
|
||||
'n_points': int(self.xyz.shape[0]),
|
||||
'shape': [int(self.xyz.shape[0]), 3],
|
||||
'raw_bytes': int(self.xyz.nbytes),
|
||||
'zlib_bytes': len(body)}, body
|
||||
if typ == 'set_target':
|
||||
sid = (hdr.get('session') or {}).get('id')
|
||||
s = self.sessions.get(sid)
|
||||
if s is None:
|
||||
return {'ok': False, 'type': 'target', 'error': '先 capture'}, b''
|
||||
pm = hdr.get('p_map')
|
||||
if not pm or len(pm) != 3:
|
||||
return {'ok': False, 'type': 'target', 'error': 'p_map 需要 3 个数'}, b''
|
||||
s['target'] = np.asarray(pm, dtype=np.float64)
|
||||
return {'ok': True, 'type': 'target',
|
||||
'target': [float(v) for v in s['target']]}, b''
|
||||
if typ == 'status':
|
||||
sid = (hdr.get('session') or {}).get('id')
|
||||
s = self.sessions.get(sid)
|
||||
target = None if s is None else s.get('target')
|
||||
out = {'ok': True, 'type': 'status',
|
||||
'has_target': target is not None,
|
||||
'valid': True, 'yaw_deg': 0.0, 'odom_age_s': 0.01,
|
||||
'odom_frame': 'odom', 'odom_child': 'imu'}
|
||||
# 模拟：人把板子往前多走了 0.25 m
|
||||
pos = np.array([0.2507, -0.0209, 0.0144])
|
||||
out['pos'] = [float(v) for v in pos]
|
||||
if target is not None:
|
||||
d = pos - target
|
||||
out['target'] = [float(v) for v in target]
|
||||
out['delta'] = [float(v) for v in d]
|
||||
return out, b''
|
||||
if typ == 'reset':
|
||||
sid = (hdr.get('session') or {}).get('id')
|
||||
s = self.sessions.get(sid)
|
||||
if s is not None:
|
||||
s['target'] = None
|
||||
return {'ok': True, 'type': 'reset'}, b''
|
||||
return {'ok': False, 'type': typ, 'error': 'unknown type'}, b''
|
||||
|
||||
def _serve(self):
|
||||
with pynng.Rep0(listen='tcp://127.0.0.1:%d' % self.port,
|
||||
recv_timeout=200) as rep:
|
||||
rep.recv_max_size = 32 * 1024 * 1024
|
||||
while not self.stop_event.is_set():
|
||||
try:
|
||||
data = rep.recv()
|
||||
except pynng.Timeout:
|
||||
continue
|
||||
except pynng.Closed:
|
||||
break
|
||||
hdr, _ = wire.unpack(data)
|
||||
reply, blob = self._handle(hdr)
|
||||
reply['session'] = hdr.get('session') or {}
|
||||
reply['server_ns'] = time.time_ns()
|
||||
rep.send(wire.pack(reply, blob))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
srv = MockServer()
|
||||
srv.start()
|
||||
print('mock server on 127.0.0.1:5599 (Ctrl+C 退出)')
|
||||
try:
|
||||
while True:
|
||||
time.sleep(1)
|
||||
except KeyboardInterrupt:
|
||||
srv.stop()
|
||||
+198
@@ -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)))
|
||||
@@ -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
|
||||
@@ -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='<f4').reshape(n, 3)
|
||||
|
||||
|
||||
def compute_target(xyz, odom, u, v, radius, fit_plane, index=None):
|
||||
"""像素 -> (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('<Button-1>', 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()
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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("<I")
|
||||
|
||||
|
||||
def new_session(board_ip, client_ip, note=""):
|
||||
"""新建一个会话（电脑端每次启动调一次）。"""
|
||||
return {
|
||||
"id": uuid.uuid4().hex,
|
||||
"client_ip": str(client_ip),
|
||||
"board_ip": str(board_ip),
|
||||
"started_ns": time.time_ns(),
|
||||
"note": str(note),
|
||||
}
|
||||
|
||||
|
||||
def pack(header, blob=b""):
|
||||
"""dict + 二进制体 -> 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"]))
|
||||
+198
@@ -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)))
|
||||
@@ -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
|
||||
@@ -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='<f4')
|
||||
body = zlib.compress(xyz.tobytes(), 1)
|
||||
rep = {
|
||||
'ok': True, 'type': 'cloud',
|
||||
'encoding': 'zlib_f32',
|
||||
'n_points': int(xyz.shape[0]),
|
||||
'shape': [int(xyz.shape[0]), 3],
|
||||
'raw_bytes': int(xyz.nbytes),
|
||||
'zlib_bytes': int(len(body)),
|
||||
'cloud_stamp_ns': int(frame['cloud_stamp_ns']),
|
||||
}
|
||||
return rep, body
|
||||
|
||||
def handle_set_target(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
if s is None or s.get('frame') is None:
|
||||
return {'ok': False, 'type': 'target', 'error': '先 capture'}, b''
|
||||
pm = hdr.get('p_map')
|
||||
if not pm or len(pm) != 3:
|
||||
return {'ok': False, 'type': 'target',
|
||||
'error': 'p_map 需要 3 个数'}, b''
|
||||
s['target'] = np.asarray([float(pm[0]), float(pm[1]), float(pm[2])],
|
||||
dtype=np.float64)
|
||||
s['target_info'] = {k: hdr.get(k) for k in ('u', 'v', 'p_cam', 'p_lidar', 'p_map')}
|
||||
s['last'] = time.monotonic()
|
||||
target = [float(v) for v in s['target']]
|
||||
return {'ok': True, 'type': 'target', 'target': target}, b''
|
||||
|
||||
def handle_status(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
od = self._odom_hist[-1] if self._odom_hist else None
|
||||
target = None if s is None else s.get('target')
|
||||
|
||||
out = {'ok': True, 'type': 'status', 'has_target': target is not None}
|
||||
if od is None:
|
||||
out.update({'valid': False, 'error': '还没有里程计'})
|
||||
return out, b''
|
||||
|
||||
age = time.monotonic() - od['recv_mono']
|
||||
pos = np.asarray(od['pos'], dtype=np.float64)
|
||||
out.update({
|
||||
'valid': bool(age <= self.args.odom_timeout),
|
||||
'pos': [float(v) for v in pos],
|
||||
'yaw_deg': float(np.rad2deg(quat_to_yaw(*od['quat']))),
|
||||
'odom_age_s': float(age),
|
||||
'odom_frame': od['frame_id'],
|
||||
'odom_child': od['child_frame_id'],
|
||||
})
|
||||
if target is not None:
|
||||
d = pos - np.asarray(target, dtype=np.float64)
|
||||
out['target'] = [float(v) for v in target]
|
||||
out['delta'] = [float(v) for v in d] # 只给坐标差，不给距离
|
||||
return out, b''
|
||||
|
||||
def handle_reset(self, hdr):
|
||||
sid = hdr['session']['id']
|
||||
with self._lock:
|
||||
s = self._sessions.get(sid)
|
||||
if s is not None:
|
||||
s['target'] = None
|
||||
s['target_info'] = None
|
||||
s['last'] = time.monotonic()
|
||||
return {'ok': True, 'type': 'reset'}, b''
|
||||
|
||||
def dispatch(self, typ, hdr):
|
||||
if typ == 'hello':
|
||||
return self.handle_hello(hdr), b''
|
||||
if typ == 'capture':
|
||||
return self.handle_capture(hdr)
|
||||
if typ == 'click':
|
||||
return self.handle_click(hdr)
|
||||
if typ == 'cloud':
|
||||
return self.handle_cloud(hdr)
|
||||
if typ == 'set_target':
|
||||
return self.handle_set_target(hdr)
|
||||
if typ == 'status':
|
||||
return self.handle_status(hdr)
|
||||
if typ == 'reset':
|
||||
return self.handle_reset(hdr)
|
||||
return {'ok': False, 'type': typ, 'error': 'unknown type'}, b''
|
||||
|
||||
# ------------------------------------------------------------------ 服务循环
|
||||
def serve(self):
|
||||
addr = 'tcp://0.0.0.0:%d' % self.args.port
|
||||
self._rep = pynng.Rep0(listen=addr)
|
||||
self._rep.recv_max_size = 32 * 1024 * 1024
|
||||
self._rep.recv_timeout = 500
|
||||
self.get_logger().info('pynng REP 监听 %s' % addr)
|
||||
|
||||
while not self._stop:
|
||||
try:
|
||||
data = self._rep.recv()
|
||||
except pynng.Timeout:
|
||||
continue
|
||||
except pynng.Closed:
|
||||
break
|
||||
except Exception as exc:
|
||||
self.get_logger().error('recv 失败: %s' % exc)
|
||||
continue
|
||||
|
||||
hdr = {}
|
||||
try:
|
||||
hdr, _ = wire.unpack(data)
|
||||
typ = hdr.get('type', '?')
|
||||
sess = hdr.get('session') or {}
|
||||
sid = sess.get('id', '?')
|
||||
reply, out_blob = self.dispatch(typ, hdr)
|
||||
reply['session'] = sess # 原样带回，含 IP
|
||||
reply['server_ns'] = time.time_ns()
|
||||
self._rep.send(wire.pack(reply, out_blob))
|
||||
self.get_logger().info(
|
||||
'[%s] %-10s from %s -> 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()
|
||||
@@ -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
|
||||
@@ -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("<I")
|
||||
|
||||
|
||||
def new_session(board_ip, client_ip, note=""):
|
||||
"""新建一个会话（电脑端每次启动调一次）。"""
|
||||
return {
|
||||
"id": uuid.uuid4().hex,
|
||||
"client_ip": str(client_ip),
|
||||
"board_ip": str(board_ip),
|
||||
"started_ns": time.time_ns(),
|
||||
"note": str(note),
|
||||
}
|
||||
|
||||
|
||||
def pack(header, blob=b""):
|
||||
"""dict + 二进制体 -> 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"]))
|
||||
Reference in New Issue
Block a user