LinkNexus/zhidian
20
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
wangjingqiao
2026-10-22 09:07:39 +08:00
commit 265f11aa38
15 changed files with 2924 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
client/_bench_index.py
+86
View File
@@ -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')
client/_mock_server.py
+198
View File
@@ -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()
client/calib.py
+198
View File
@@ -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)))
client/requirements.txt
+7
View File
@@ -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
client/session_client.py
+572
View File
@@ -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()
client/target_math.py
+234
View File
@@ -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
client/test_session.py
+83
View File
@@ -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()
client/wire.py
+65
View File
@@ -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"]))